udpvisionlistener.java

基于网络的AIBO机器狗遥操作控制程序代码

import java.awt.image.BufferedImage;
import java.util.Vector;
import java.util.Date;

import java.io.InputStream;
import java.io.ByteArrayInputStream;

import java.net.DatagramSocket;
import java.net.DatagramPacket;
import java.awt.image.BufferedImage;
import java.awt.image.Raster;
import java.awt.image.IndexColorModel;
import java.util.Date;
import javax.imageio.ImageIO;
import javax.imageio.ImageReader;
import javax.imageio.stream.ImageInputStream;
import javax.imageio.stream.MemoryCacheImageInputStream;

public class UDPVisionListener extends UDPListener implements VisionListener {
  DatagramSocket mysock;
	
  InputStream in;

	boolean updatedFlag;
	Date timestamp;
	long frameNum=0;

	protected Vector listeners = new Vector();
	protected boolean updating;
	
	public void addListener(VisionUpdatedListener l) { listeners.add(l); needConnection(); }
	public void removeListener(VisionUpdatedListener l) { listeners.remove(l); }
	public void fireVisionUpdate() {
		updating=true;
		//System.out.println("==>UDPVisionListener::fireVisionUpdated() called===listeners.size "+listeners.size());
		for(int i=0; i<listeners.size() && updating; i++)
			((VisionUpdatedListener)listeners.get(i)).visionUpdated(this);
		updating=false;
	}

	public Date getTimeStamp() { return timestamp; }
	public long getFrameNum() { return frameNum; }
	public IndexColorModel getColorModel() { return cmodel; }

	public boolean hasData() {
		return updatedFlag;
	}
 
	public boolean isConnected() {
		return _isConnected;
	}

	int channels=3;
	int width=DEFAULT_WIDTH;
	int height=DEFAULT_HEIGHT;
	int pktSize=width*height*channels;
	int oldformat=PACKET_VISIONRAW_FULL;
	int format;
	int compression;
	int chan_id;

	byte[] _data=new byte[pktSize];
	byte[] _outd=new byte[pktSize];
	byte[] _tmp=new byte[pktSize*2];
	byte[] _jpeg=new byte[pktSize*2];
	byte[] _newjpeg=new byte[pktSize*2];
	boolean isJPEG=false;
	int jpegLen=0;
	int newjpegLen=0;
	boolean isIndex=false;
	boolean badCompressWarn=false;
	int[] _pixels=new int[width*height];
	BufferedImage img=new BufferedImage(width,height,BufferedImage.TYPE_INT_RGB);
	int bytesRead;
	boolean convertRGB=true;
	IndexColorModel cmodel;

	public boolean hasRawJPEG() { return isJPEG; }
	public byte[] getJPEG() { return _jpeg; }
	public int getJPEGLen() { return jpegLen; }
	
	public String readLoadSaveString(InputStream in) throws java.io.IOException {
		int creatorLen=readInt(in);
		if(!_isConnected) return ""; //System.out.println("creatorLen=="+creatorLen);
		String creator=new String(readBytes(in,creatorLen));
		if(!_isConnected) return "";
		if(readChar(in)!='\0')
			System.err.println("Misread LoadSave string? "+creator);
		return creator;
	}

	public boolean readChannel(InputStream in, int c, int chanW, int chanH) throws java.io.IOException {
		readBytes(_tmp,in,chanW*chanH);
		if(!_isConnected) return false;
		return upsampleData(c,chanW,chanH);
	}
	public boolean upsampleData(int c, int chanW, int chanH) {
		if(chanW==width && chanH==height) {
			//special case: straight copy if image and channel are same size
			for(int y=0;y<height;y++) {
				int datarowstart=y*width*channels+c;
				int tmprowstart=y*chanW;
				for(int x=0;x<width;x++)
					_data[datarowstart+x*channels]=_tmp[tmprowstart+x];
			}
			return true;
		}
		//otherwise this channel is subsampled, need to blow it up
		//we'll linearly interpolate between pixels
		//METHOD A:
		//hold edges, interpolate through middle:
		//  if we have 2 samples, scaling up to 4
		//   index: 0   1    2   3
		// maps to: 0  1/3  2/3  1
		/*
		float xsc=(chanW-1)/(float)(width-1);
		float ysc=(chanH-1)/(float)(height-1);
		for(int y=0;y<height-1;y++) {
			int datarowstart=y*width*channels+c;
			float ty=y*ysc;
			int ly=(int)ty; //lower pixel index
			float fy=ty-ly; //upper pixel weight
			int tmprowstart=ly*chanW;
			for(int x=0;x<width-1;x++) {
				float tx=x*xsc;
				int lx=(int)tx; //lower pixel index
				float fx=tx-lx; //upper pixel weight

				float lv=((int)_tmp[tmprowstart+lx]&0xFF)*(1-fx)+((int)_tmp[tmprowstart+lx+1]&0xFF)*fx;
				float uv=((int)_tmp[tmprowstart+lx+chanW]&0xFF)*(1-fx)+((int)_tmp[tmprowstart+lx+1+chanW]&0xFF)*fx;
				_data[datarowstart+x*channels]=(byte)(lv*(1-fy)+uv*fy);
			}
			_data[datarowstart+(width-1)*channels]=_tmp[tmprowstart+chanW-1];
		}
		int datarowstart=width*(height-1)*channels+c;
		int tmprowstart=chanW*(chanH-1);
		for(int x=0;x<width-1;x++) {
			float tx=x*xsc;
			int lx=(int)tx; //lower pixel index
			float fx=tx-lx; //upper pixel weight
			_data[datarowstart+x*channels]=(byte)(((int)_tmp[tmprowstart+lx]&0xFF)*(1-fx)+((int)_tmp[tmprowstart+lx+1]&0xFF)*fx);
		}
		_data[datarowstart+(width-1)*channels]=_tmp[tmprowstart+chanW-1];
		*/
		
		//Unfortunately, pixels are simply interleaved, starting at the
		//top right.  So, Method A will stretch things to the bottom-right
		//a bit.  This method holds left edge and spacing, so it lines up
		//better with what's being transmitted (but the bottom right edges
		//wind up smeared)
		//METHOD B:
		//  if we have 2 samples, scaling up to 4
		//   index: 0   1    2   3
		// maps to: 0  1/2   1   1  <-- this last one would be 3/2, so we have to replicate 1
		float xsc=(chanW)/(float)(width);
		float ysc=(chanH)/(float)(height);
		int xgap=Math.round(1.0f/xsc);
		int ygap=Math.round(1.0f/ysc);
		for(int y=0;y<height-ygap;y++) {
			int datarowstart=y*width*channels+c;
			float ty=y*ysc;
			int ly=(int)ty; //lower pixel index
			float fy=ty-ly; //upper pixel weight
			int tmprowstart=ly*chanW;
			for(int x=0;x<width-xgap;x++) {
				float tx=x*xsc;
				int lx=(int)tx; //lower pixel index
				float fx=tx-lx; //upper pixel weight

				float lv=(_tmp[tmprowstart+lx]&0xFF)*(1-fx)+(_tmp[tmprowstart+lx+1]&0xFF)*fx;
				float uv=(_tmp[tmprowstart+lx+chanW]&0xFF)*(1-fx)+(_tmp[tmprowstart+lx+1+chanW]&0xFF)*fx;
				_data[datarowstart+x*channels]=(byte)(lv*(1-fy)+uv*fy);
			}
			for(int x=width-xgap;x<width;x++) {
				float lv=(_tmp[tmprowstart+chanW-1]&0xFF);
				float uv=(_tmp[tmprowstart+chanW-1+chanW]&0xFF);
				_data[datarowstart+x*channels]=(byte)(lv*(1-fy)+uv*fy);
			}
		}
		for(int y=height-ygap;y<height;y++) {
			int datarowstart=y*width*channels+c;
			int tmprowstart=chanW*(chanH-1);
			for(int x=0;x<width-xgap;x++) {
				float tx=x*xsc;
				int lx=(int)tx; //lower pixel index
				float fx=tx-lx; //upper pixel weight

				float lv=(_tmp[tmprowstart+lx]&0xFF)*(1-fx)+(_tmp[tmprowstart+lx+1]&0xFF)*fx;
				_data[datarowstart+x*channels]=(byte)(lv);
			}
			for(int x=width-xgap;x<width;x++)
				_data[datarowstart+x*channels]=_tmp[tmprowstart+chanW-1];
		}
		
		return true;
	}

	public boolean readJPEGChannel(InputStream in, int c, int chanW, int chanH) throws java.io.IOException {
		int len=readInt(in);
		newjpegLen=len;
		//System.out.println("len="+len);
		if(!_isConnected) return false;
		if(len>=_newjpeg.length) {
			System.out.println("Not enough tmp room");
			return false;
		}
		readBytes(_newjpeg,in,len);
		if(!_isConnected) return false;
		if(len>chanW*chanH*channels) {
			if(!badCompressWarn) {
				badCompressWarn=true;
				System.out.println("Compressed image is larger than raw would be... :(");
			}
		} else {
			if(badCompressWarn) {
				badCompressWarn=false;
				System.out.println("...ok, compressed image is smaller than raw now... :)");
			}
		}

		try {
			ImageInputStream jpegStream=new MemoryCacheImageInputStream(new ByteArrayInputStream(_newjpeg));
			jpegReader.setInput(jpegStream); 
			Raster decoded=jpegReader.readRaster(0,null);
			int off=c;
			for(int y=0; y<chanH; y++)
				for(int x=0; x<chanW; x++) {
					_data[off]=(byte)decoded.getSample(x,y,0);
					off+=channels;
				}
		} catch(Exception ex) { ex.printStackTrace(); }
		return true;
	}

	public boolean readJPEG(InputStream in, int chanW, int chanH) throws java.io.IOException {
		int len=readInt(in);
		newjpegLen=len;
		//System.out.println("len="+len);
		if(!_isConnected) return false;
		if(len>=_newjpeg.length) {
			System.out.println("Not enough tmp room");
			return false;
		}
		readBytes(_newjpeg,in,len);
		if(!_isConnected) return false;
		if(len>chanW*chanH*channels) {
			if(!badCompressWarn) {
				badCompressWarn=true;
				System.out.println("Compressed image is larger than raw would be... :(");
			}
		} else {
			if(badCompressWarn) {
				badCompressWarn=false;
				System.out.println("...ok, compressed image is smaller than raw now... :)");
			}
		}

		try {
			ImageInputStream jpegStream=new MemoryCacheImageInputStream(new ByteArrayInputStream(_newjpeg));
			jpegReader.setInput(jpegStream); 
			Raster decoded=jpegReader.readRaster(0,null);
			int off=0;
			for(int y=0; y<chanH; y++)
				for(int x=0; x<chanW; x++) {
					_data[off++]=(byte)decoded.getSample(x,y,0);
					_data[off++]=(byte)decoded.getSample(x,y,2);
					_data[off++]=(byte)decoded.getSample(x,y,1);
				}
		} catch(Exception ex) { ex.printStackTrace(); }
		return true;
	}

	byte[] colormap = new byte[256*3];
	public boolean readColorModel(InputStream in) throws java.io.IOException {
		int len=readInt(in);
		//System.out.println("len="+len);
		if(!_isConnected) return false;
		readBytes(colormap,in,len*3);
		if(!_isConnected) return false;
		//we'll do this stupid thing because we can't change an existing color model, and we don't want to make a new one for each frame
		// (btw, java is stupid)
		boolean makeNew=false;
		if(cmodel==null || len!=cmodel.getMapSize()) {
			makeNew=true;
		} else {
			int off=0;
			for(int i=0; i<len; i++) {
				if((byte)cmodel.getRed(i)!=colormap[off++] || (byte)cmodel.getGreen(i)!=colormap[off++] || (byte)cmodel.getBlue(i)!=colormap[off++]) {
					makeNew=true;
					break;
				}
			}
		}
		if(makeNew) {
			//System.out.println("new color model");
			cmodel=new IndexColorModel(7,len,colormap,0,false);
		}
		return true;
	}
	
	public boolean readIndexedColor(InputStream in, int chanW, int chanH) throws java.io.IOException {
		readBytes(_data,in,chanW*chanH);
		if(!_isConnected) return false;
		if(!readColorModel(in)) return false;
		if(!_isConnected) return false;
		isIndex=true;
		return true;
	}

	public boolean readRLE(InputStream in, int chanW, int chanH) throws java.io.IOException {
		int len=readInt(in);
		if(!_isConnected) return false;
		readBytes(_tmp,in,len*5);
		if(!_isConnected) return false;

		int dpos=0;
		int curx=0, cury=0;
		for (; len>0 && cury<chanH;) {
			byte color=_tmp[dpos++];
			int x=((int)_tmp[dpos++]&0xFF);
			x|=((int)_tmp[dpos++]&0xFF)<<8;
			int rlen=((int)_tmp[dpos++]&0xFF);
			rlen|=((int)_tmp[dpos++]&0xFF)<<8;
			//System.out.println(color + " "+x + " "+rlen);
			len--;
			if (x < curx)
				return false;
			
			for (; curx < x; curx++)
				_data[cury*width+curx]=0;
			
			if (curx+rlen>width)
				return false;
			
			for (; rlen>0; rlen--, curx++)
				_data[cury*width+curx]=color;
			if (curx==width) {
				cury++;
				curx=0;
			}
		}
		if(!readColorModel(in)) return false;
		if(!_isConnected) return false;
		isIndex=true;
		return true;
	}

	
	public boolean readRegions(InputStream in, int chanW, int chanH) throws java.io.IOException {
		//clear the _data array