/**
* Draw a string at a given point.
*
* <p>The method returns the point where the next string should be drawn to seamlessly continue
* the current string.
*
* @param s the string
* @param point the position where to draw the string
* @param fontname the font's name, should preferably be a logical name
* @param fontsize the font size
* @param color the color
* @return the point where outputting string text should continue
*/
public Point drawString(String s, Point point, String fontname, int fontsize, Color color) {
Graphics g = this.image.getGraphics();
g.setColor(color);
g.setFont(new Font(fontname, Font.PLAIN, fontsize));
g.drawString(s, point.x, point.y);
FontMetrics f = g.getFontMetrics();
Point newPoint = new Point(point);
newPoint.x += f.stringWidth(s);
this.repaint();
return (newPoint);
}
public void paint(Graphics g) {
Graphics2D g2 = (Graphics2D) g;
Dimension asz = this.getSize();
if (fullRefresh) {
g2.clearRect(0, 0, asz.width, asz.height);
fullRefresh = false;
}
g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
big.setColor(Color.black);
offset.x = (int) (asz.width - iw) / 2;
offset.y = (int) (asz.height - ih) / 2;
big.drawImage(img, 0, 0, this);
big.setPaint(Color.red);
if ((rect.width > 0) && (rect.height > 0)) big.draw(rect);
if (selected == 1) shadeExt(big, 0, 0, 0, 64);
else if (selected == 2) {
shadeExt(big, 0, 0, 0, 255);
selected = 1;
}
g2.drawImage(bi, offset.x, offset.y, this);
}
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");
}
}
}
}
}
