public void handleImage() {
while (true) {
Image src = null;
try {
src = new Image(visionImage.take(), width, height);
} catch (InterruptedException e) {
e.printStackTrace();
continue;
}
// Apply Blob Track to Image
Image dest = new Image(src);
blobTrack.apply(src, dest);
OdometryMsg msg = new OdometryMsg();
if (LOCALIZE && System.currentTimeMillis() - last_localization > LOCALIZATION_INTERVAL) {
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = false;
stopPub.publish(stop_msg);
Point2D.Double curr_point;
synchronized (localization) {
curr_point = localization.localize(odo_x, odo_y, blobTrack.fiducials);
}
msg.x = curr_point.x;
msg.y = curr_point.y;
last_localization = System.currentTimeMillis();
} else {
msg.x = odo_x;
msg.y = odo_y;
}
msg.theta = odo_theta;
localPub.publish(msg);
// Update newly formed vision message
gui.setVisionImage(dest.toArray(), width, height);
try {
Thread.sleep(1000);
} catch (Exception exc) {
exc.printStackTrace();
}
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = true;
stopPub.publish(stop_msg);
}
}
