/* * JohnRobotSim.java *//* This code is part of the abstractrobot package of TeamBots. * Copyright (c) 1999, 2000 by John Sweeney and Carnegie Mellon University */package EDU.cmu.cs.coral.abstractrobot;import java.awt.*;import java.util.*;import EDU.gatech.cc.is.util.*;import EDU.gatech.cc.is.simulation.*;import EDU.gatech.cc.is.abstractrobot.VisualObjectSensor;import EDU.gatech.cc.is.abstractrobot.Simple;import EDU.gatech.cc.is.abstractrobot.SimpleInterface;import EDU.gatech.cc.is.communication.Message;import EDU.cmu.cs.coral.simulation.ColorTransitionSim;import EDU.cmu.cs.coral.simulation.LandmarkSim;import EDU.cmu.cs.coral.simulation.LineSim;import EDU.cmu.cs.coral.localize.VisionRobot;import EDU.cmu.cs.coral.localize.LocalizationRobot;import EDU.cmu.cs.coral.localize.LineLocalizationRobot;import EDU.cmu.cs.coral.localize.Sample;import EDU.cmu.cs.coral.localize.Range;import EDU.cmu.cs.coral.localize.GaussianSampler;import EDU.cmu.cs.coral.localize.DoubleRectangle;/** this is an implementation of some localization using the SimpleCyeSim * as a base.  there are a few things needed for localization not present * in a the standard Sim, such as particular heading information routines, * communication, and some other stuff. * * * @see SimpleCyeSim * @author John Sweeney * @version 0.1 */public class JohnCyeSim extends SimpleCyeSim implements SimulatedObject,							SimpleInterface,							VisualObjectSensor,							LocalizationRobot,							LineLocalizationRobot{    protected Point last;    protected LandmarkSim [] landmarks; //keep track of these  //  protected SimulatedObject [] all_objects; //and the rest too   protected LineSim [] lines; //these are the lines on the field...    protected double desired_heading; //which way we want to go    public Color foreground, background; //how to be drawn       protected long time; //our time    protected long lastTimeIncrement;    protected double timed; //more accurate?    protected double base_speed;    protected double desired_speed;    protected double desired_turret_heading;    protected	double	obstacle_rangeM = 1.0;      protected	long	last_VisualObjectst = 0;    protected Vec2[]	last_VisualObjects;    protected	int	num_VisualObjects = 0;    protected int last_channel = 0;  protected boolean [] ambig; // the landmarks which are ambiguous  protected int numAmbigLandmark;  protected Vec2 [] linesRes; //result from getVisualLines  protected int horzRes; //horizontal scanning resoluation (in parts) for looking for lines  protected int vertRes; //vert scannig rez for same (in parts)  protected long lastVisualLinesTime;  protected int lastChannel;  protected double lineRes; //the resoluation for sacnning (in meters)     protected	long	last_Obstaclest = 0;    protected	Vec2	last_Obstacles[];    protected	int	num_Obstacles;    protected DoubleRectangle [] landmarkRectangle;  //  protected Vec2 [] fieldPos;  //  protected Vec2 [] fieldMag;  //  protected Color [] fieldColor;  //  protected Color singleVectorColor;  //  protected boolean haveField;  //  protected boolean vectorsHaveColor;  //  protected Vec2 estPos;        protected double oldMoveR, oldMoveT, oldHeading;  protected String display_string = "";//"this string left blank";    protected double clippingOffset;    protected double clippingStdDev;    protected double clippingDirStdDev;    protected GaussianSampler gs;   protected Random intGen;  //  public static final double RADIUS = 0.247; //this will change for cye   //  public static final double VISION_RANGE = 1.5;  //    public static final int VISION_FOV_DEG = 100;  //   public static final double VISION_FOV_RAD = Units.DegToRad(100);    public final double MAX_STEER = 0.7854;    public final double MAX_TURRET = 0.7854;    public final double MAX_TRANSLATION = 0.508;    public final boolean DEBUG = false;    public JohnCyeSim() {	super();		lastTimeIncrement =0;	landmarks = null;	lines = null;	clippingStdDev = 0.25;	clippingOffset = 0.5;	clippingDirStdDev = 0.098; //4 degrees	gs  = new GaussianSampler(3); //used in clipToMap	gs.setRange(2, -Math.PI, Math.PI);	for (int i =0; i < 3; i++) {	  gs.setMean(i, 0.0);	  gs.setDev(i, clippingStdDev);	}	gs.setDev(2, clippingDirStdDev);		oldMoveR = oldMoveT = oldHeading = 0.0;	intGen = new Random();    }    public void init(double x, double y, double t, double ignore,		     Color fg, Color bg, int vc, int id, long s) {             super.init(x, y, t, ignore, fg, bg, vc, id, s);	last = new Point(0,0);		landmarks = null;	lines = null;	linesRes = null;	//	lineRes = getDictionary().getDouble("LINE_LOCALIZER_RESOLUTION"); 	lineRes = 0.05;	horzRes = 20;	vertRes = 20;    }            // public void receive(Message m) {/*N/A*/}     public void drawID(Graphics g, int w, int h, double t, double b,		       double l, double r) {		top =t; bottom =b; left =l; right =r;		if (DEBUG) System.out.println("draw "+				      w + " " +				      h + " " +				      t + " " +				      b + " " +				      l + " " +				      r + " ");	double meterspp = (r - l) / (double)w;	int radius = (int)(RADIUS / meterspp);	int xpix = (int)((position.x - l) / meterspp);	int ypix = (int)((double)h - ((position.y - b) / meterspp));		/*--- draw ID ---*/	g.setColor(background);	//	g.drawString("JohnCyeSim!",	//     xpix-radius,ypix-radius);	/*	int xx, yy;	if (linesRes != null) {	  for (int i =0;i < linesRes.length; i+= 2) {	    if (linesRes[i] != null) {	      //this is ego centric....	      Vec2 globTo = new Vec2(linesRes[i]);	      Vec2 globFrom = new Vec2(linesRes[i+1]);	      	      globFrom.add(position);	      globTo.add(position);	      	      //start of line	      xpix = (int) ( (globFrom.x - l) / meterspp);	      xx = (int) ( (globTo.x - l) / meterspp);	      	      ypix = (int) ( (double)h - ((globFrom.y - b) / meterspp));	      yy = (int) ( (double)h - ((globTo.y - b) / meterspp));	      	      g.drawLine(xpix, ypix, xx, yy);	    }	  }	}	*/	  	    }      public void takeStep(long time_increment, SimulatedObject [] all_objs) {    //need to get the landmarks, and call it here because of initialization    //sequence...    // getLandmarks();    super.takeStep(time_increment, all_objs);    //System.out.println("mvstep.r = "+mvstep.r+" t = "+mvstep.t);    getLandmarks();    getLines();  }        //now for the ones in SimpleInterface not covered in SimulatedObject        /* these are specific to LocalizationRobot*/  public SimulatedObject[] getLandmarks() {    if (landmarks == null) {      int i;      int num=0;      //landmarks = new SimulatedObject[all_objects.length];            int ambigChannel = getDictionary().getInt("LOCALIZER_AMBIG_LM_CHANNEL");      numAmbigLandmark = 0;      for (i = 0; i < all_objects.length; i++) {	if (all_objects[i] instanceof LandmarkSim ||	    all_objects[i] instanceof ColorTransitionSim) {  	  System.out.println("GL: obj "+i+" is lm! getP = "+			     all_objects[i].getPosition().toString());	  // landmarks[i] = all_objects[i];	  num++;	  if (all_objects[i].getVisionClass() == ambigChannel) {	    numAmbigLandmark++;	  }	}	else {	  System.out.println("GL: obj "+i+" is not lm...");	  //    landmarks[i] = null;	}      }            landmarks = new LandmarkSim[num];      ambig = new boolean[num];           for (i = 0, num = 0 ; i < all_objects.length; i++) {	if (all_objects[i] instanceof LandmarkSim ||	    all_objects[i] instanceof ColorTransitionSim) {	  landmarks[num] = (LandmarkSim) all_objects[i];	  	  //check if ambi landmark, and if so put in array	  if (all_objects[i].getVisionClass() == ambigChannel) {	    ambig[num] = true;	  }	  	  num++;	}      }            System.out.println("JRS: getLandmarks: got "+num+" landmarks");            landmarkRectangle = new DoubleRectangle[landmarks.length];            for (i =0; i < landmarkRectangle.length; i++) {	double newx, newy, side;	newx= (double)landmarks[i].getPosition().x - landmarks[i].getRadius();	newy = (double) landmarks[i].getPosition().y + landmarks[i].getRadius();	side = landmarks[i].getRadius()*2.0;	landmarkRectangle[i] = new DoubleRectangle(newx, newy, side, side);      }					           }    return landmarks;  }    public boolean [] getAmbigLandmarks() { return ambig; }     public int getNumLandmarks() {	if (landmarks == null)	    getLandmarks();	return landmarks.length;    }    public double getSeenLandmarkConfidence(int lm) {      //if the landmark is in visual range 100% conf, else 0%      double noise = visionNoiseGetNext();      Vec2 temp = landmarks[lm].getCenter(position);      //dont add noise to whether we see the landmark      /*      temp.setr((1.0 - noise ) * temp.r);      noise = visionNoiseGetNext();      temp.sett((1.0 - noise) * temp.t);      */      if ((temp.r < SimpleCye.VISION_RANGE) &&	    (Math.abs(Units.BestTurnRad(steer.t,temp.t))	     < (SimpleCye.VISION_FOV_RAD/2.0))) {		if (ambig[lm]) {	  return 1.0 / (double)(numAmbigLandmark);	}		return 1.0;      }	      return 0.0;    }          public double getLandmarkDistance(int lm) {           Vec2 tmp = new Vec2(landmarks[lm].getPosition());            tmp.sub(position);            //add some noise to this....assumption is mean is 0      double noise = visionNoiseGetNext();      tmp.setr( (1.0-noise)*tmp.r);            return tmp.r;    }  //make this next one a param from the dsc file?  public double getLandmarkDistanceVariance(int lm) {    return getDictionary().getDouble("LOCALIZER_LM_DIST_VAR");  }    public double getLandmarkAngle(int lm) {    Vec2 tmp = new Vec2(landmarks[lm].getPosition());    double noise =  visionNoiseGetNext(); //make it noisy       tmp.sub(position);    tmp.sett((1.0-noise)*tmp.t);