Skip to content

Commit 721867a

Browse files
lopsided98lopsided98
committed
Add UDP stream viewer.
1 parent 85dee41 commit 721867a

File tree

2 files changed

+201
-1
lines changed

2 files changed

+201
-1
lines changed

‎src/main/java/org/usfirst/frc/team2084/smartdashboard/extensions/vision/UDPStreamViewerExtension.java

+200
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,200 @@
1+
package org.usfirst.frc.team2084.smartdashboard.extensions.vision;
2+
3+
import java.awt.Color;
4+
import java.awt.Dimension;
5+
import java.awt.Graphics;
6+
import java.awt.Graphics2D;
7+
import java.awt.geom.AffineTransform;
8+
import java.awt.image.BufferedImage;
9+
import java.io.ByteArrayInputStream;
10+
import java.net.DatagramPacket;
11+
import java.net.DatagramSocket;
12+
import java.net.InetAddress;
13+
import java.net.SocketException;
14+
import java.net.UnknownHostException;
15+
16+
import javax.imageio.ImageIO;
17+
18+
import org.usfirst.frc.team2084.CMonster2016.vision.UDPVideoServer;
19+
import org.usfirst.frc.team2084.CMonster2016.vision.VisionParameters;
20+
21+
import edu.wpi.first.smartdashboard.gui.StaticWidget;
22+
import edu.wpi.first.smartdashboard.properties.IntegerProperty;
23+
import edu.wpi.first.smartdashboard.properties.Property;
24+
25+
/**
26+
*
27+
* @author Ben Wolsieffer
28+
*/
29+
@SuppressWarnings("serial")
30+
public class UDPStreamViewerExtension extends StaticWidget {
31+
32+
public static final String NAME = "UDP Stream Viewer";
33+
34+
private double rotateAngleRad = 0;
35+
private long lastFPSCheck = 0;
36+
private int lastFPS = 0;
37+
private int fpsCounter = 0;
38+
39+
private DatagramSocket socket;
40+
private final byte[] buffer = new byte[100000];
41+
42+
public class BGThread extends Thread {
43+
44+
boolean destroyed = false;
45+
46+
public BGThread() {
47+
super("Camera Viewer Background");
48+
}
49+
50+
long lastRepaint = 0;
51+
52+
@Override
53+
public void run() {
54+
while (!destroyed) {
55+
try {
56+
DatagramPacket packet = new DatagramPacket(buffer, buffer.length);
57+
socket.receive(packet);
58+
59+
fpsCounter++;
60+
long currTime = System.currentTimeMillis();
61+
long elapsedTime = currTime - lastFPSCheck;
62+
if (elapsedTime > 500) {
63+
if(elapsedTime > 2000) {
64+
updateIP();
65+
}
66+
lastFPSCheck = currTime;
67+
lastFPS = fpsCounter * 2;
68+
fpsCounter = 0;
69+
}
70+
71+
lastRepaint = System.currentTimeMillis();
72+
ByteArrayInputStream tmpStream = new ByteArrayInputStream(buffer, 0, packet.getLength());
73+
imageToDraw = ImageIO.read(tmpStream);
74+
75+
imageUpdated(imageToDraw);
76+
77+
repaint();
78+
79+
} catch (Exception e) {
80+
imageToDraw = null;
81+
repaint();
82+
e.printStackTrace();
83+
}
84+
}
85+
86+
}
87+
88+
@Override
89+
public void destroy() {
90+
destroyed = true;
91+
}
92+
}
93+
94+
private BufferedImage imageToDraw;
95+
private BGThread bgThread = new BGThread();
96+
public final IntegerProperty rotateProperty = new IntegerProperty(this, "Degrees Rotation", 0);
97+
98+
/**
99+
*
100+
*/
101+
public UDPStreamViewerExtension() {
102+
try {
103+
socket = new DatagramSocket(UDPVideoServer.PORT);
104+
} catch (SocketException e) {
105+
e.printStackTrace();
106+
}
107+
}
108+
109+
private void updateIP() {
110+
try {
111+
VisionParameters.setStreamIP(InetAddress.getLocalHost().getHostAddress());
112+
} catch (UnknownHostException e) {
113+
System.out.println(e);
114+
}
115+
}
116+
117+
@Override
118+
public void init() {
119+
setPreferredSize(new Dimension(320, 240));
120+
propertyChanged(rotateProperty);
121+
bgThread.start();
122+
updateIP();
123+
revalidate();
124+
repaint();
125+
}
126+
127+
@Override
128+
public void propertyChanged(Property property) {
129+
if (property == rotateProperty) {
130+
rotateAngleRad = Math.toRadians(rotateProperty.getValue());
131+
}
132+
133+
}
134+
135+
@Override
136+
public void disconnect() {
137+
bgThread.destroy();
138+
super.disconnect();
139+
}
140+
141+
@Override
142+
protected void paintComponent(Graphics g) {
143+
BufferedImage drawnImage = imageToDraw;
144+
145+
if (drawnImage != null) {
146+
// cast the Graphics context into a Graphics2D
147+
Graphics2D g2d = (Graphics2D) g;
148+
149+
// get the existing Graphics transform and copy it so that we can
150+
// perform scaling and rotation
151+
AffineTransform origXform = g2d.getTransform();
152+
AffineTransform newXform = (AffineTransform) (origXform.clone());
153+
154+
// find the center of the original image
155+
int origImageWidth = drawnImage.getWidth();
156+
int origImageHeight = drawnImage.getHeight();
157+
int imageCenterX = origImageWidth / 2;
158+
int imageCenterY = origImageHeight / 2;
159+
160+
// perform the desired scaling
161+
double panelWidth = getBounds().width;
162+
double panelHeight = getBounds().height;
163+
double panelCenterX = panelWidth / 2.0;
164+
double panelCenterY = panelHeight / 2.0;
165+
double rotatedImageWidth = origImageWidth * Math.abs(Math.cos(rotateAngleRad)) + origImageHeight * Math.abs(Math.sin(rotateAngleRad));
166+
double rotatedImageHeight = origImageWidth * Math.abs(Math.sin(rotateAngleRad)) + origImageHeight * Math.abs(Math.cos(rotateAngleRad));
167+
168+
// compute scaling needed
169+
double scale = Math.min(panelWidth / rotatedImageWidth, panelHeight / rotatedImageHeight);
170+
171+
// set the transform before drawing the image
172+
// 1 - translate the origin to the center of the panel
173+
// 2 - perform the desired rotation (rotation will be about origin)
174+
// 3 - perform the desired scaling (will scale centered about
175+
// origin)
176+
newXform.translate(panelCenterX, panelCenterY);
177+
newXform.rotate(rotateAngleRad);
178+
newXform.scale(scale, scale);
179+
g2d.setTransform(newXform);
180+
181+
// draw image so that the center of the image is at the "origin";
182+
// the transform will take care of the rotation and scaling
183+
g2d.drawImage(drawnImage, -imageCenterX, -imageCenterY, null);
184+
185+
// restore the original transform
186+
g2d.setTransform(origXform);
187+
188+
g.setColor(Color.PINK);
189+
g.drawString("FPS: " + lastFPS, 10, 10);
190+
} else {
191+
g.setColor(Color.PINK);
192+
g.fillRect(0, 0, getBounds().width, getBounds().height);
193+
g.setColor(Color.BLACK);
194+
g.drawString("NO CONNECTION", 10, 10);
195+
}
196+
}
197+
198+
public void imageUpdated(BufferedImage image) {
199+
}
200+
}

‎src/main/java/org/usfirst/frc/team2084/smartdashboard/extensions/vision/VisionExtension.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
* @author Ben Wolsieffer
2626
*/
2727
@SuppressWarnings("serial")
28-
public abstract class VisionExtension extends MJPEGStreamViewerExtension {
28+
public abstract class VisionExtension extends UDPStreamViewerExtension {
2929

3030
static {
3131
try {

0 commit comments

Comments
 (0)