BallDetector(boolean _display) {
ctx = Freenect.createContext();
if (ctx.numDevices() > 0) {
kinect = ctx.openDevice(0);
} else {
System.err.println("WARNING: No kinects detected");
return;
}
display = _display;
controlFrame = new JFrame("Controls");
controlFrame.setLayout(new GridLayout(5, 1));
controlFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
pg = new ParameterGUI();
pg.addIntSlider("maxDepth", "max depth", 800, 2047, 1050);
pg.addIntSlider("blobThresh", "blob thresh", 1, 500, 125);
pg.addIntSlider("thresh", "thresh", 1, 100, 10);
pg.addIntSlider("frames", "frames", 1, 1000, 1);
pg.addListener(
new ParameterListener() {
public void parameterChanged(ParameterGUI _pg, String name) {
if (name.equals("thresh")) {
KinectDepthVideo.THRESH = _pg.gi(name);
} else if (name.equals("frames")) {
KinectDepthVideo.MAX_FRAMES = _pg.gi(name);
} else if (name.equals("maxDepth")) {
KinectDepthVideo.MAX_DEPTH = _pg.gi(name);
}
}
});
controlFrame.add(pg, 0, 0);
startTracking = new JButton("Start Tracking Balls");
startTracking.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
if (!tracking) {
tracking = true;
colorStream.pause();
depthStream.pause();
startTracking.setText("Stop Tracking");
} else {
tracking = false;
colorStream.resume();
depthStream.resume();
startTracking.setText("Start Tracking Balls");
}
}
});
controlFrame.add(startTracking, 1, 0);
resetProjectile = new JButton("Reset Projectile");
resetProjectile.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
try {
lcm.publish("6_RESET", "reset");
} catch (IOException ex) {
System.out.println("can't publish reset");
}
}
});
controlFrame.add(resetProjectile, 2, 0);
resetDepth = new JButton("Reset Depth Avgs");
resetDepth.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
DepthClearer ic = new DepthClearer(pg);
ic.start();
}
});
controlFrame.add(resetDepth, 3, 0);
JPanel scoreButtons = new JPanel(new GridLayout(1, 3));
JButton addHuman = new JButton("human++");
addHuman.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
try {
lcm.publish("6_SCORE_HUMAN", "bish");
} catch (IOException ex) {
System.out.println("can't publish score");
}
}
});
JButton addRobot = new JButton("robot++");
addRobot.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
try {
lcm.publish("6_SCORE_ROBOT", "bish");
} catch (IOException ex) {
System.out.println("can't publish score");
}
}
});
JButton resetScores = new JButton("reset scores");
resetScores.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent e) {
try {
lcm.publish("6_SCORE_RESET", "bish");
} catch (IOException ex) {
System.out.println("can't publish score");
}
}
});
scoreButtons.add(addHuman, 0, 0);
scoreButtons.add(addRobot, 0, 1);
scoreButtons.add(resetScores, 0, 2);
controlFrame.add(scoreButtons, 4, 0);
controlFrame.setSize(800, 600);
controlFrame.setVisible(true);
colorFrame = new JFrame("color feed");
colorMonitor = new Object();
colorStream = new KinectRGBVideo(kinect, colorMonitor, display);
colorFrame.setDefaultCloseOperation(JFrame.DO_NOTHING_ON_CLOSE);
colorFrame.addWindowListener(new RGBClose());
colorFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT);
colorFrame.setContentPane(colorStream);
colorFrame.setVisible(true);
depthFrame = new JFrame("depth feed");
depthMonitor = new Object();
depthStream = new KinectDepthVideo(kinect, depthMonitor, display);
depthFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
depthFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT);
depthFrame.setContentPane(depthStream);
depthFrame.setVisible(true);
rgbImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB);
depthImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB);
validImageValue = new boolean[KinectVideo.WIDTH * KinectVideo.HEIGHT];
try {
lcm = new LCM("udpm://239.255.76.67:7667?ttl=1");
} catch (IOException e) {
lcm = LCM.getSingleton();
}
BALL = new Statistics();
finder = new BallTracker(KinectVideo.WIDTH, KinectVideo.HEIGHT, false);
if (display) {
depthImg = depthStream.getFrame();
rgbImg = colorStream.getFrame();
}
// get robot position from click
depthStream.addMouseListener(
new MouseAdapter() {
public void mouseClicked(MouseEvent e) {
Point botPix = e.getPoint();
botStart = depthStream.getWorldCoords(botPix);
botStart.z += 0.08;
System.out.println("botStart: " + botStart.toString());
depthStream.showSubtraction();
depthStream.botLoc = botPix;
}
});
DepthClearer ic = new DepthClearer(pg);
ic.start();
}
public void run() {
while (true) {
BALL = null;
ball_t ballLCM = new ball_t();
synchronized (depthMonitor) {
try {
depthMonitor.wait();
} catch (Exception e) {
e.printStackTrace();
}
}
ballLCM.nanoTime = depthStream.latestTimestamp;
ArrayList<Statistics> blobs;
depthStream.getReadLock().lock();
try {
blobs = finder.analyze2(depthStream.getValidImageArray());
} finally {
depthStream.getReadLock().unlock();
}
Statistics ball = null;
Statistics robot = null;
int minSize = pg.gi("blobThresh");
// find robot and ball by blob size
Collections.sort(blobs, ComparatorFactory.getStatisticsCompareSize());
// find robot and ball by y pixel
// Collections.sort(blobs, ComparatorFactory.getStatisticsCompareYPix());
// if (tracking) {
// System.out.println("num blobs: " + blobs.size());
// System.out.println("biggest blob size: " + blobs.get(0).N);
// }
// for (Statistics blob : blobs) {
// if (blob.N > 10) {
// System.out.println("blob size: " + blob.N);
// }
// else {
// break;
// }
// }
if (blobs.size() == 1) {
Statistics first = blobs.get(0);
if (first.N > minSize) {
ball = first;
}
} else if (blobs.size() >= 2) {
Statistics first = blobs.get(0);
Statistics second = blobs.get(1);
if (first.N > minSize) {
ball = first;
}
if (second.N > minSize) {
robot = first;
ball = second;
}
}
// System.out.println("balls points " + depthStream.trajectory.size());
// if not tracking keep kv.depthStream.trajectory to just one index
if (!tracking) {
depthStream.trajectory.clear();
}
if (ball != null) {
depthStream.trajectory.add(ball);
Point depthPix = ball.center();
Point depthCoord = new Point();
depthCoord.x = depthPix.x - KinectVideo.C_X;
depthCoord.y = KinectVideo.C_Y - depthPix.y;
// System.out.println("avg depth " + ball.Uz());
// get depth from average depth of blob
double realDepth = raw_depth_to_meters(ball.Uz());
Point3D coord = depthStream.getWorldCoords(depthCoord, realDepth);
if (depthPix != null) {
for (int y = depthPix.y - 3; y < depthPix.y + 3; y++) {
for (int x = depthPix.x - 3; x < depthPix.x + 3; x++) {
try {
depthImg.setRGB(x, y, 0xFFFF0000);
} catch (Exception e) {
// System.out.println(x + " " + y);
}
;
}
}
// if (tracking) {
// //save image
// depthStream.getReadLock().lock();
// try {
// File imgFile = new File("image" + ballNum++ + ".png");
// try {
// ImageIO.write(depthImg, "png", imgFile);
// }
// catch(Exception e) {
// System.out.println("can't save img");
// }
// }
// finally {
// depthStream.getReadLock().unlock();
// }
// }
if (tracking) {
ballLCM.x = coord.x;
ballLCM.y = coord.y;
ballLCM.z = coord.z;
// if(tracking)
System.out.println("updating new ball (" + System.currentTimeMillis() + ")");
// if (ballLCM.x > CatchController.TARGET_MAX_X)
lcm.publish("6_BALL", ballLCM);
// else
// System.out.println("ball past target zone");
}
}
}
}
}