udpvisionlistener.java

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

		for(int h=0; h<chanH; h++) {
			for(int w =0; w<chanW; w++) {
				_data[chanW*h+w]=0;
			}
		}
  
		int numColors=readInt(in);
		if(!_isConnected) return false;
		for(int curColor = 0; curColor < numColors; curColor++) {
			int numRegions = readInt(in);
			if(!_isConnected) return false;
            
			readBytes(_tmp,in,36*numRegions);
			if(!_isConnected) return false;
            	
			int dpos=0;
			for (int i = 0; i<numRegions; i++) {
				byte color= _tmp[dpos];
				dpos +=4;
				int x1 = byteToInt(_tmp,dpos);
				dpos +=4;
				int y1 = byteToInt(_tmp,dpos);
				dpos +=4;
				int x2 = byteToInt(_tmp,dpos);
				dpos +=4;
				int y2 = byteToInt(_tmp,dpos);
				//The  data of the centroid, area and run_start are ignored
				dpos +=20; //isn't nessescary, but now it nicely adds up to 36
				if (  x1 > chanW || y1 > chanH || x2 > chanW || y2 > chanH
							|| x1 > x2 || y1 > y2
							|| x1 < 0 || x2 < 0 || y1 < 0 || y2 < 0
							)
					return false;
				
				//Fill the data array with the bounding boxes..
				//..the top and bottom lines
				for (int tb = x1; tb <= x2; tb++) {
					_data[y1*width+tb]=color;
					_data[y2*width+tb]=color;
				}
				//..the left and right lines
				for (int lr = y1; lr <= y2; lr++) {
					_data[lr*width+x1]=color;
					_data[lr*width+x2]=color;
				}
			}
			readBytes(_tmp,in,12); //read out the min_area, total_area and merge_threshhold and ignore them:)
		}
		if(!readColorModel(in)) return false;
		if(!_isConnected) return false;
		isIndex=true;
		return true;
	}


	public void connected(DatagramSocket UDPsocket, DatagramPacket incoming) {
		_isConnected=true;
		mysock = UDPsocket;
		while(listeners==null) {
			System.out.println("Assert: Bad race condition -- shouldn't be happening");
			Thread.yield();
		}
		fireVisionUpdate();
		try {
			while(!mysock.isClosed()) {
				in = new ByteArrayInputStream(incoming.getData());
				String type = readLoadSaveString(in);
				if(!_isConnected) break; //System.out.println("Got type="+type);
				if(!type.equals("TekkotsuImage")) {
					if(!type.equals("CloseConnection"))
						System.err.println("Unrecognized type: "+type);
					else {
						System.out.println("Got Close connection packet");
						_isConnected=false;
					}
					break;
				}
        fireConnected();
				format=readInt(in);
				if(!_isConnected) break; //System.out.println("Got format="+format);
				compression=readInt(in);
				if(!_isConnected) break; //System.out.println("Got compression="+compression);
				int newWidth=readInt(in);
				if(!_isConnected) break; //System.out.println("Got newWidth="+newWidth);
				int newHeight=readInt(in);
				if(!_isConnected) break; //System.out.println("Got newHeight="+newHeight);
				long timest=readInt(in);
				if(timest<0)
					timest+=(1L<<32);
				if(!_isConnected) break; //System.out.println("Got timest="+timest);
				frameNum=readInt(in);
				if(frameNum<0)
					frameNum+=(1L<<32);
				if(!_isConnected) break; //System.out.println("Got frameNum="+frameNum);
				
				if (format!=oldformat || newWidth!=width || newHeight!=height) {
					width=newWidth;
					height=newHeight;
					synchronized (_outd) {
						switch (format) {
						case ENCODE_COLOR:
							channels=3;
							pktSize=width*height*channels;
							break;
						case ENCODE_SINGLE_CHANNEL:
							channels=1;
							pktSize=width*height*channels;
							break;
						default:
							System.err.println("VisionRawListener: unknown packet type "+format);
							throw new java.lang.NoSuchFieldException("fake exception");
						}
						_data=new byte[pktSize];
						_outd=new byte[pktSize];
						_tmp=new byte[pktSize];
						_jpeg=new byte[pktSize*2<2000?2000:pktSize*2];
						_newjpeg=new byte[pktSize*2<2000?2000:pktSize*2];
						_pixels=new int[width*height];;
						img=new BufferedImage(width,height,BufferedImage.TYPE_INT_RGB);
						oldformat=format;
					}
				}
				
				boolean failed=false;
				for(int i=0; i<channels; i++) {
					String creator = readLoadSaveString(in);
					if(!_isConnected) break; //System.out.println("Got creator="+creator);
					if(!creator.equals("FbkImage")) {
						System.err.println("Unrecognized creator: "+creator);
						failed=true; break;
					} else {
						int chanwidth=readInt(in);
						if(!_isConnected) break; //System.out.println("Got chanwidth="+chanwidth);
						int chanheight=readInt(in);
						if(!_isConnected) break; //System.out.println("Got chanheight="+chanheight);
						
						if(chanwidth>width || chanheight>height) {
							System.err.println("channel dimensions exceed image dimensions");
							failed=true; break;
						}
						
						int layer=readInt(in);
						if(!_isConnected) break; //System.out.println("Got layer="+layer);
						chan_id=readInt(in);
						if(!_isConnected) break; //System.out.println("Got chan_id="+chan_id);
				
						String fmt=readLoadSaveString(in);
						if(!_isConnected) break; //System.out.println("Got fmt="+fmt);
						if(fmt.equals("blank")) {
							isJPEG=false;
							isIndex=false;
							int useChan=(channels==1)?i:chan_id;
							int off=useChan;
							for(int y=0; y<height; y++)
								for(int x=0; x<width; x++) {
									_data[off]=(byte)(convertRGB?0x80:0);
									off+=channels;
								}
						} else if(fmt.equals("RawImage")) {
							isJPEG=false;
							isIndex=false;
							int useChan=(channels==1)?i:chan_id;
							if(!readChannel(in,useChan,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener channel read failed"); break; }
						} else if(fmt.equals("JPEGGrayscale")) {
							isIndex=false;
							int useChan=(channels==1)?i:chan_id;
							if(!readJPEGChannel(in,useChan,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener JPEGGreyscale channel read failed"); break; }
							isJPEG=(channels==1);
						} else if(fmt.equals("JPEGColor")) {
							isIndex=false;
							if(format==ENCODE_SINGLE_CHANNEL)
								System.err.println("WTF? ");
							if(!readJPEG(in,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener JPEGColor channel read failed"); break; }
							i=channels;
							isJPEG=true;
						} else if(fmt.equals("SegColorImage")) {
							isJPEG=false;
							isIndex=true;
							if(!readIndexedColor(in,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener SegColor read failed"); break; }
						} else if(fmt.equals("RLEImage")) {
							isJPEG=false;
							isIndex=true;
							if(!readRLE(in,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener RLEImage read failed"); break; }
						} else if(fmt.equals("RegionImage")) {
							isJPEG=false;
							isIndex=true;
							if(!readRegions(in,chanwidth,chanheight)) { failed=true; System.err.println("UDPVisionListener RegionImage read failed"); break; }
						} else {
							isJPEG=false;
							isIndex=false;
							System.err.println("Unrecognized format: "+fmt);
							failed=true; break;
						}
					}
				}
				if(failed || !_isConnected) {
					System.err.println("UDPVisionListener connection lost");
					break;
				}
				
				synchronized(_outd) {
					byte[] temp=_data;
					_data=_outd;
					_outd=temp;
					if(isJPEG) {
						temp=_newjpeg;
						_newjpeg=_jpeg;
						_jpeg=temp;
						int tempi=newjpegLen;
						newjpegLen=jpegLen;
						jpegLen=tempi;
					}
					timestamp=new Date(timest);
				}
				updatedFlag=true;
				fireVisionUpdate();
				mysock.receive(incoming);
			}
		} catch (Exception ex) {
    } finally {
      fireDisconnected();
    }
		
		try { mysock.close(); } catch (Exception ex) { }
		if(!_isConnected) {
			if(!destroy)
				System.out.println("Waiting 5 seconds to reconnect");
			try { Thread.sleep(5000); } catch (Exception ex) {}
		}
		_isConnected=false;
		fireVisionUpdate();
	}
	
	public byte[] getData() {
		synchronized (_outd) {
			updatedFlag=false;
			return _outd;
		}
	}

	public void setConvertRGB(boolean b) { 
		if(b!=convertRGB) {
			convertRGB=b;
			updatedFlag=true;
		}
	}
	public boolean getConvertRGB() { return convertRGB; }

	public int[] getYUVPixels() {
		synchronized(_outd) {
			byte[] data=getData();
			int offset=0;
			for (int i=0; i<width*height; i++) {
				int y=(int)data[offset++]&0xFF;
				int u=(int)data[offset++]&0xFF;
				int v=(int)data[offset++]&0xFF;
				_pixels[i]=(255<<24) | (y<<16) | (u<<8) | v;
			}
		}
		return _pixels;
	}

	//This still uses RGB pixels just in case you want to display the
	//intensity value into hues instead of black/white
	public int[] getIntensityPixels() {
		synchronized(_outd) {
			byte[] data=getData();
			int offset=0;
			if(!getConvertRGB()) {
				for (int i=0; i<width*height; i++) {
					int z=(int)data[offset++]&0xFF;
					_pixels[i]=(255<<24) | (z<<16) | (z<<8) | z;
				}
			} else if(chan_id==CHAN_Y || chan_id==CHAN_Y_DY || chan_id==CHAN_Y_DX || chan_id==CHAN_Y_DXDY ) {
				for (int i=0; i<width*height; i++) {
					int z=(int)data[offset++]&0xFF;
					_pixels[i]=pixelYUV2RGB(z,0x80,0x80);
				}
			} else if(chan_id==CHAN_U) {
				for (int i=0; i<width*height; i++) {
					int z=(int)data[offset++]&0xFF;
					_pixels[i]=pixelYUV2RGB(0x80,z,0x80);
				}
			} else if(chan_id==CHAN_V) {
				for (int i=0; i<width*height; i++) {
					int z=(int)data[offset++]&0xFF;
					_pixels[i]=pixelYUV2RGB(0x80,0x80,z);
				}
			}
		}		
		return _pixels;
	}

	public int[] getRGBPixels() {
		synchronized(_outd) {
			byte[] data=getData();
			int offset=0;
			for (int i=0; i<width*height; i++) {
				int y=(int)data[offset++]&0xFF;
				int u=(int)data[offset++]&0xFF;
				int v=(int)data[offset++]&0xFF;
				_pixels[i]=pixelYUV2RGB(y, u, v);
			}
		}
		return _pixels;
	}

	static final int pixelYUV2RGB(int y, int u, int v) {
		u=u*2-255;
		v=v*2-255;
		int r=y+u;
		int b=y+v;
		u=u>>1;
		v=(v>>2)-(v>>4);
		int g=y-u-v;
		if (r<0) r=0; if (g<0) g=0; if (b<0) b=0;
		if (r>255) r=255; if (g>255) g=255; if (b>255) b=255;

		return (255<<24) | (r<<16) | (g<<8) | b;
	}

	public BufferedImage getImage() {
		if(!updatedFlag)
			return img;
		if(isIndex) {
			byte[] data=getData();
			if(cmodel==null)
				return img;
			if(img.getWidth()!=width || img.getHeight()!=height || img.getType()!=BufferedImage.TYPE_BYTE_INDEXED)
				img=new BufferedImage(width,height,BufferedImage.TYPE_BYTE_INDEXED,cmodel);
			img.getRaster().setDataElements(0,0,width,height,data);
		} else {
			updatedFlag=false;
			int[] pix;
			if(img.getWidth()!=width || img.getHeight()!=height || img.getType()!=BufferedImage.TYPE_BYTE_INDEXED)
				img=new BufferedImage(width,height,BufferedImage.TYPE_INT_RGB);
			if(format==ENCODE_COLOR)
				pix=getConvertRGB()?getRGBPixels():getYUVPixels();
			else
				pix=getIntensityPixels();
			img.setRGB(0,0,width,height,pix,0,width);
		}
		return img;
	}



	public UDPVisionListener() { super(); }
	public UDPVisionListener(int port) { super(port); }
	public UDPVisionListener(String host, int port) { super(host,port); }
}