simplecyesim.java

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

			{
			body[j].setr(body[j].r / meterspp);
			body[j].sett(body[j].t - steer.t); 
			bodyx[j] = (int)body[j].x + xpix;
			bodyy[j] = (int)body[j].y + ypix;
			}
		g.fillPolygon(bodyx, bodyy, 4);
		int dirx = xpix + (int)((double)radius * Math.cos(steer.t));
		int diry = ypix + -(int)((double)radius * Math.sin(steer.t));
		g.setColor(background);
		/*--- draw steer      ---*/
		g.drawArc(xpix - visionr, ypix - visionr,
				visionr + visionr, visionr + visionr,
				(int)Units.RadToDeg(steer.t) 
				- (SimpleCye.VISION_FOV_DEG/2), SimpleCye.VISION_FOV_DEG);

		dirx = xpix + (int)((double)radius * Math.cos(steer.t)*0.5);
		diry = ypix + -(int)((double)radius * Math.sin(steer.t)*0.5);
		g.drawLine(xpix, ypix, dirx, diry);

		/*--- draw trailer ---*/

		int txpix = (int)((trailer_position.x - l) / meterspp);
		int typix = (int)((double)h - ((trailer_position.y - b) / meterspp));


		Vec2[] tbody = new Vec2[5];     // outline of trailer
		int[] tbodyx = new int[5];
		int[] tbodyy = new int[5];
		tbody[0] = new Vec2(TRAILER_FRONT, 0); 
		tbody[1] = new Vec2(-1*TRAILER_FRONT, TRAILER_WIDTH/2);
		tbody[2] = new Vec2(-1*TRAILER_LENGTH, TRAILER_WIDTH/2);
		tbody[3] = new Vec2(-1*TRAILER_LENGTH, -1*TRAILER_WIDTH/2);
		tbody[4] = new Vec2(-1*TRAILER_FRONT,  -1*TRAILER_WIDTH/2);
		for(int k = 0; k<5; k++) // scale and rotate
			{
			tbody[k].setr(tbody[k].r / meterspp);
			tbody[k].sett(tbody[k].t - trailer_steer.t);
			tbodyx[k] = (int)tbody[k].x + txpix;
			tbodyy[k] = (int)tbody[k].y + typix;
			}
		g.fillPolygon(tbodyx, tbodyy, 5);

		}


	/**
	 * Clean up.
	 */
	public void quit()
		{
		}


	/**
	 * Gets time elapsed since the robot was instantiated. 
	 * Since this is simulation, it may not match real elapsed time.
	 */
	public long getTime()
		{
		return(time);
		}


	private	long	last_Obstaclest = 0;
	private	Vec2	last_Obstacles[];
	private	int	num_Obstacles;
	/**
	 * Get an array of Vec2s that point egocentrically from the
	 * center of the robot to the obstacles currently sensed by the 
	 * bumpers and sonars.
	 * @param timestamp only get new information 
	 *	if timestamp > than last call or timestamp == -1 .
	 * @return the sensed obstacles.
	 */
	public Vec2[] getObstacles(long timestamp)
		{
		if((timestamp > last_Obstaclest)||(timestamp == -1))
			{
			if (timestamp != -1) last_Obstaclest = timestamp;
			Vec2 tmp_objs[] = new Vec2[all_objects.length];
			num_Obstacles = 0;
			/*--- check all objects ---*/
			for(int i = 0; i<all_objects.length; i++)
				{
				/*--- check if it's an obstacle and not self ---*/
				if (all_objects[i].isObstacle() &&
					(all_objects[i].getID() != unique_id))
					{
					Vec2 tmp = all_objects[i].getClosestPoint(
							position);
					if (tmp.r<obstacle_rangeM)
					tmp_objs[num_Obstacles++] = tmp;
					}
				}
			last_Obstacles = new Vec2[num_Obstacles];
			for(int i = 0; i<num_Obstacles; i++)
			last_Obstacles[i] = new Vec2(tmp_objs[i].x,
				tmp_objs[i].y);
			}
		Vec2[] retval = new Vec2[num_Obstacles];
		for(int i = 0; i<num_Obstacles; i++)
			retval[i] = new Vec2(last_Obstacles[i].x,
				last_Obstacles[i].y);
		return(retval);
		}


	private	double	obstacle_rangeM = 1.0;
	/**
	 * Set the maximum range at which a sensor reading should be considered
	 * an obstacle.  Beyond this range, the readings are ignored.
	 * The default range on startup is 1 meter.
	 * @param range the range in meters.
	 */
	public void setObstacleMaxRange(double range)
		{
		obstacle_rangeM = range;
		}


	private	long	last_VisualObjectst = 0;
	private Vec2[]	last_VisualObjects;
	private	int	num_VisualObjects = 0;
	private	int	last_channel = 0;
	/**
	 * Get an array of Vec2s that represent the
	 * locations of visually sensed objects egocentrically
 	 * from center of the robot to the objects currently sensed by the 
	 * vision system.	
	 * @param timestamp only get new information 
	 *	if timestamp > than last call or timestamp == -1 .
	 * @param channel (1-6) which type/color of object to retrieve.
	 * @return the sensed objects.
	 */
	public Vec2[] getVisualObjects(long timestamp, int channel)
		{
		if(((timestamp > last_VisualObjectst)||
			(channel != last_channel))||(timestamp == -1))
			{
			if (timestamp != -1) last_VisualObjectst = timestamp;
			last_channel = channel;
			num_VisualObjects = 0;
			Vec2 tmp_objs[] = new Vec2[all_objects.length];
			/*--- check all objects ---*/
			for(int i = 0; i<all_objects.length; i++)
				{
				/*--- check if it's of the right code and not self ---*/
				if (all_objects[i].getVisionClass()==channel &&
					(all_objects[i].getID() != unique_id))
					{
					Vec2 tmp = all_objects[i].getClosestPoint(
							position);
					//add some noise to our vision.  in this case, 
					//change the value of tmp by a noisy amount
					//this is a 0-meaned distribution
					double noise = visionNoiseGetNext();
					
					//the new value is x% of the old one
					tmp.setr( tmp.r * (1.0 - noise));

					//get new random part for theta
					noise = visionNoiseGetNext();
					tmp.sett( tmp.t* (1.0 - noise));
					

					if ((tmp.r<SimpleCye.VISION_RANGE)&&
						(Math.abs(
						Units.BestTurnRad(steer.t,tmp.t))
						< (SimpleCye.VISION_FOV_RAD/2)))
						tmp_objs[num_VisualObjects++] = tmp;
					}
				}
			last_VisualObjects = new Vec2[num_VisualObjects];
			for(int i = 0; i<num_VisualObjects; i++)
			last_VisualObjects[i] = new Vec2(tmp_objs[i].x,
				tmp_objs[i].y);
			}
		Vec2[] retval = new Vec2[num_VisualObjects];
		for(int i = 0; i<num_VisualObjects; i++)
			retval[i] = new Vec2(last_VisualObjects[i].x,
				last_VisualObjects[i].y);
		return(retval);
		}

	  /**
	    * This sets the variance of the normal distribution used to
	    * simulate noise in the vision sensor.  It also gives a seed
	    * value, which allows for repeatable pseudo-noise.
	    * @param mean the mean of the distribution.  
	    * @param stddev this is the standard deviation.  It is defined for values >= 0.
	    * a vlaue of 0 means noise does not affect the sensor.
	    * @param seed this is the seed value for the random number generator.
	    */
	  public void setVisionNoise(double mean, double stddev, long seed) {
	    visionNoiseGen = new GaussianSampler(1, seed);
	    visionNoiseStddev = stddev;
	    visionNoiseMean = mean;
	  }
	    
	  protected double visionNoiseGetNext() {
	    //this is a mean =0 gaussian
	    return visionNoiseGen.generateDist(visionNoiseMean, visionNoiseStddev); 
	  }

	private	long	last_VisualSizest = 0;
	/**
	 * NOT IMPLEMENTED:
	 * Get an array of doubles that represent an estimate of the
	 * size in square meters of the visually sensed objects.
	 * @param timestamp only get new information if 
	 * 	timestamp > than last call or timestamp == -1 .
	 * @param channel (1-6) which type/color of object to retrieve.
	 * @return the sizes of the sensed objects.
	 */
	public double[] getVisualSizes(long timestamp, int channel)
		{
		/* todo */
		return(new double[0]);
		}


	private	long	last_VisualAxest = 0;
	/**
	 * NOT IMPLEMENTED:
	 * Get an array of doubles that represent the
	 * major axis orientation of the visually sensed objects.
	 * 0 and PI are horizontal, PI/2 is vertical.
	 * @param timestamp only get new information 
	 *	if timestamp > than last call or timestamp == -1 .
	 * @param channel (1-6) which type/color of object to retrieve.
	 * @return the major axes of the sensed objects.
	 */
	public double[] getVisualAxes(long timestamp, int channel)
		{
		/* todo */
		return(new double[0]);
		}




	/**
	 * Get the position of the robot in global coordinates.
	 * @param timestamp only get new information 
	 *	if timestamp > than last call or timestamp == -1.
	 * @return the position.
	 */
	public Vec2 getPosition(long timestamp)
		{
		return(new Vec2(position.x, position.y));
		}
		

	/**
	 * Get the position of the robot in global coordinates.
	 * @return the position.
	 */
	public Vec2 getPosition()
		{
		return(new Vec2(position.x, position.y));
		}
		

	/**
	 * Reset the odometry of the robot in global coordinates.
	 * This might be done when reliable sensor information provides
	 * a very good estimate of the robot's location, or if you
	 * are starting the robot in a known location other than (0,0).
	 * Do this only if you are certain you're right!
	 * @param position the new position.
	 */
	public void resetPosition(Vec2 posit)
		{
		position.setx(posit.x);
		position.sety(posit.y);
		}


	private boolean	in_reverse = false;
	/**
	*/
	public double getSteerHeading(long timestamp)
		{
		return(steer.t);
		}

	
	/**
	*/
	public void resetSteerHeading(double heading)
		{
		/* ensure in legal range */
		heading = Units.ClipRad(heading); 

		/* if we're in reverse, the steer heading is PI out */
		if (in_reverse) heading = Units.ClipRad(heading + Math.PI);

		/* set the angle */
		steer.sett(heading);
		}


	private double desired_heading;
	/**
	*/
	public void setSteerHeading(long timestamp, double heading)
		{
		/* ensure in legal range */
		desired_heading = Units.ClipRad(heading);

		/* check if we should go in reverse */
/*
		double turn = Units.BestTurnRad(steer.t, desired_heading);
		if (Math.abs(turn)>(Math.PI/2))
			{
			in_reverse = true;
			desired_heading = 
				Units.ClipRad(desired_heading+Math.PI);
			}
		else in_reverse = false;
*/
		}
	

	/**
	*/
	
	private double desired_speed = 0;
	/**
	*/ 
	public void setSpeed(long timestamp, double speed)
		{
		/* ensure legal range */
		if (speed > 1.0) speed = 1.0;
		else if (speed < -1.0) speed = -1.0; // allowing backing up
		desired_speed = speed;
		}


	protected double base_speed = MAX_TRANSLATION;
	/**
	*/
	public void setBaseSpeed(double speed)
		{
		if (speed > MAX_TRANSLATION) speed = MAX_TRANSLATION;
		else if (speed < 0) speed = 0;
		base_speed = speed;
		}


        private long    last_Opponentst = 0;
        private Vec2    last_Opponents[];
        /**
         * Get an array of Vec2s that point egocentrically from the
         * center of the robot to the opponents currently sensed by the
         * robot
         * @param timestamp only get new information if 
	 *		timestamp > than last call or timestamp == -1 .
         * @return the sensed Opponents.
         */
        public Vec2[] getOpponents(long timestamp)
                {
                if((timestamp > last_Opponentst)||(timestamp == -1))
                        {
                        if (timestamp != -1) last_Opponentst = timestamp;
                        last_Opponents = kin_sensor.getOpponents(all_objects);
                        }
                return(last_Opponents);
                }


        /**
         * Return an int represting the player's ID on the team.
         * This value may not be valid if the simulation has not
         * been "set up" yet.  Do not use it during initialization.
         * @param timestamp only get new information if
         *      timestamp > than last call or timestamp == -1 .
         * @return the number.
         */
        public int getPlayerNumber(long timestamp)
                {
                return(kin_sensor.getPlayerNumber(all_objects));
                }


        private long    last_Teammatest = 0;
        private Vec2    last_Teammates[] = new Vec2[0];
        /**
         * Get an array of Vec2s that point egocentrically from the
         * center of the robot to the teammates currently sensed by the
         * robot.
         * @param timestamp only get new information if
         *      timestamp > than last call or timestamp == -1 .
         * @return the sensed teammates.
         */
        public Vec2[] getTeammates(long timestamp)
                {
                if((timestamp > last_Teammatest)||(timestamp == -1))
                        {
                        if (timestamp != -1) last_Teammatest = timestamp;
                        last_Teammates = kin_sensor.getTeammates(all_objects);
                        }
                return(last_Teammates);
                }


        private double  kin_rangeM = 4.0;
        /**
         * Set the maximum range at which a sensor reading should be considered
         * kin.  Beyond this range, the readings are ignored.
         * Also used by opponent sensor.
         * The default range on startup is 1 meter.
         * @param range the range in meters.
         */
        public void setKinMaxRange(double range)
                {
                kin_rangeM = range;
                kin_sensor.setKinMaxRange(range);
                }

        public Color getForegroundColor() { return foreground; }
        public Color getBackgroundColor() { return background; }



        /*--- Transceiver methods ---*/


        public void multicast(int[] ids, Message m)
                throws CommunicationException
                {
                transceiver.multicast(ids, m, all_objects);
                }

        public void broadcast(Message m)
                {
                transceiver.broadcast(m, all_objects);
                }

        public void unicast(int id, Message m)
                throws CommunicationException
                {
                transceiver.unicast(id, m, all_objects);
                }

        public CircularBufferEnumeration getReceiveChannel()
                {
                return(transceiver.getReceiveChannel());
                }

        public void setCommunicationMaxRange(double m)
                {
                transceiver.setCommunicationMaxRange(m);
                }

        public void receive(Message m)
                {
                transceiver.receive(m);
                }

        public boolean connected()
                {
                return(true);
                }
	}