johncyesim.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

/*
 * 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);