diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CameraServer.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CameraServer.java index f01fe38825..3024508b61 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CameraServer.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CameraServer.java @@ -1,6 +1,7 @@ package edu.wpi.first.wpilibj; import java.io.DataInputStream; +import java.io.DataOutputStream; import java.io.IOException; import java.io.OutputStream; import java.net.InetSocketAddress; @@ -8,8 +9,11 @@ import java.net.ServerSocket; import java.net.Socket; import java.nio.ByteBuffer; import java.nio.ByteOrder; +import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Deque; import java.util.List; +import java.util.NoSuchElementException; import java.util.concurrent.atomic.AtomicBoolean; import com.ni.vision.NIVision; @@ -17,262 +21,343 @@ import com.ni.vision.NIVision.Image; import com.ni.vision.NIVision.RawData; import com.ni.vision.VisionException; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.vision.USBCamera; + //replicates CameraServer.cpp in java lib public class CameraServer { - public static CameraServer getInstance() { - if (server == null) { - server = new CameraServer(); - } - return server; - } + private static final int kPort = 1180; + private static final byte[] kMagicNumber = { 0x01, 0x00, 0x00, 0x00 }; + private static final int kSize640x480 = 0; + private static final int kSize320x240 = 1; + private static final int kSize160x120 = 2; + private static final int kHardwareCompression = -1; + private static final String kDefaultCameraName = "cam1"; + private static final int kMaxImageSize = 200000; + private static CameraServer server; - private static CameraServer server; - private AtomicBoolean ready; - private Thread serverThread; - private int m_quality; - private static final int kPort = 1180; - private static final byte[] kMagicNumber = { 0x01, 0x00, 0x00, 0x00 }; - private static final int kSize640x480 = 0; - private static final int kSize320x240 = 1; - private static final int kSize160x120 = 2; - private static final int kHardwareCompression = -1; - private static final String kDefaultCameraName = "cam1"; - private List m_imageData; - private boolean m_autoCaptureStarted; + public static CameraServer getInstance() { + if (server == null) { + server = new CameraServer(); + } + return server; + } - private CameraServer() { - ready = new AtomicBoolean(false); - m_imageData = new ArrayList(); - m_quality = 50; - serverThread = new Thread(new Runnable() { - public void run() { - try { - serve(); - } catch (IOException e) { - // do stuff here - } catch (InterruptedException e) { - // do stuff here - } - } - }); - serverThread.start(); - } + private Thread serverThread; + private int m_quality; + private boolean m_autoCaptureStarted; + private boolean m_hwClient = true; + private USBCamera m_camera; + private CameraData m_imageData; + private Deque m_imageDataPool; - /** - * Manually change the image that is served by the MJPEG stream. This can be - * called to pass custom annotated images to the dashboard. Note that, for - * 640x480 video, this method could take between 40 and 50 milliseconds to - * complete. - * - * This shouldn't be called if {@link #startAutomaticCapture} is called. - * - * @param image - * The IMAQ image to show on the dashboard - */ - public synchronized void setImage(Image image) { - // handle multi-threadedness + private class CameraData { + RawData data; + int start; + public CameraData(RawData d, int s) { + data = d; + start = s; + } + } - /* Flatten the IMAQ image to a JPEG */ + private CameraServer() { + m_quality = 50; + m_camera = null; + m_imageData = null; + m_imageDataPool = new ArrayDeque<>(3); + for (int i = 0; i < 3; i++) { + m_imageDataPool.addLast(ByteBuffer.allocateDirect(kMaxImageSize)); + } + serverThread = new Thread(new Runnable() { + public void run() { + try { + serve(); + } catch (IOException e) { + // do stuff here + } catch (InterruptedException e) { + // do stuff here + } + } + }); + serverThread.setName("CameraServer Send Thread"); + serverThread.start(); + } - RawData data = NIVision.imaqFlatten(image, - NIVision.FlattenType.FLATTEN_IMAGE, - NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality); - ByteBuffer buffer = data.getBuffer(); - m_imageData.clear(); + private synchronized void setImageData(RawData data, int start) { + if (m_imageData != null && m_imageData.data != null) { + m_imageData.data.free(); + if (m_imageData.data.getBuffer() != null) + m_imageDataPool.addLast(m_imageData.data.getBuffer()); + m_imageData = null; + } + m_imageData = new CameraData(data, start); + notifyAll(); + } - /* Find the start of the JPEG data */ - int index = 0; - while (index < buffer.limit() - 1) { - if ((buffer.get(index) & 0xff) == 0xFF - && (buffer.get(index + 1) & 0xff) == 0xD8) - break; - index++; - } - while (index < buffer.limit()) { - m_imageData.add(buffer.get(index++)); - } + /** + * Manually change the image that is served by the MJPEG stream. This can be + * called to pass custom annotated images to the dashboard. Note that, for + * 640x480 video, this method could take between 40 and 50 milliseconds to + * complete. + * + * This shouldn't be called if {@link #startAutomaticCapture} is called. + * + * @param image + * The IMAQ image to show on the dashboard + */ + public void setImage(Image image) { + // handle multi-threadedness - if (m_imageData.size() <= 2) { - throw new VisionException( - "data size of flattened image is less than 2. Try another camera! "); - } + /* Flatten the IMAQ image to a JPEG */ - data.free(); + RawData data = NIVision.imaqFlatten(image, + NIVision.FlattenType.FLATTEN_IMAGE, + NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality); + ByteBuffer buffer = data.getBuffer(); + boolean hwClient; - ready.set(true); - } + synchronized (this) { + hwClient = m_hwClient; + } - /** - * Start automatically capturing images to send to the dashboard. - * - * You should call this method to just see a camera feed on the dashboard - * without doing any vision processing on the roboRIO. {@link #setImage} - * shouldn't be called after this is called. - * - * @param cameraName - * The name of the camera interface (e.g. "cam1") - */ - public void startAutomaticCapture(String cameraName) { - synchronized (this) { - if (m_autoCaptureStarted) { - // print - // "you fucked up \"Automatic capture has already been started\"" - return; - } - m_autoCaptureStarted = true; - } + /* Find the start of the JPEG data */ + int index = 0; + if (hwClient) { + while (index < buffer.limit() - 1) { + if ((buffer.get(index) & 0xff) == 0xFF + && (buffer.get(index + 1) & 0xff) == 0xD8) + break; + index++; + } + } - CaptureRunnable runnable = new CaptureRunnable(cameraName); - Thread captureThread = new Thread(runnable); - captureThread.start(); + if (buffer.limit() - index - 1 <= 2) { + throw new VisionException("data size of flattened image is less than 2. Try another camera! "); + } - } + setImageData(data, index); + } - private class CaptureRunnable implements Runnable { - String name; + /** + * Start automatically capturing images to send to the dashboard. + * You should call this method to just see a camera feed on the dashboard + * without doing any vision processing on the roboRIO. {@link #setImage} + * shouldn't be called after this is called. + * This overload calles {@link #startAutomaticCapture(String)} with the + * default camera name + */ + public void startAutomaticCapture() { + startAutomaticCapture(USBCamera.kDefaultCameraName); + } - public CaptureRunnable(String name) { - this.name = name; - } + /** + * Start automatically capturing images to send to the dashboard. + * + * You should call this method to just see a camera feed on the dashboard + * without doing any vision processing on the roboRIO. {@link #setImage} + * shouldn't be called after this is called. + * + * @param cameraName + * The name of the camera interface (e.g. "cam1") + */ + public void startAutomaticCapture(String cameraName) { + USBCamera camera = new USBCamera(cameraName); + camera.openCamera(); + startAutomaticCapture(camera); + } - @Override - public void run() { - Image frame = NIVision.imaqCreateImage( - NIVision.ImageType.IMAGE_RGB, 0); - int id = NIVision.IMAQdxOpenCamera(name, - NIVision.IMAQdxCameraControlMode.CameraControlModeController); - NIVision.IMAQdxConfigureGrab(id); - NIVision.IMAQdxStartAcquisition(id); + public synchronized void startAutomaticCapture(USBCamera camera) { + if (m_autoCaptureStarted) return; + m_autoCaptureStarted = true; + m_camera = camera; - while (true) { - NIVision.IMAQdxGrab(id, frame, 1); - setImage(frame); - } + m_camera.startCapture(); - // NIVision.IMAQdxStopAcquisition(id); - // NIVision.IMAQdxCloseCamera(id); + Thread captureThread = new Thread(new Runnable() { + @Override + public void run() { + capture(); + } + }); + captureThread.setName("Camera Capture Thread"); + captureThread.start(); + } - } + protected void capture() { + Image frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0); + while (true) { + boolean hwClient; + ByteBuffer dataBuffer = null; + synchronized (this) { + hwClient = m_hwClient; + if (hwClient) { + dataBuffer = m_imageDataPool.removeLast(); + } + } - } + try { + if (hwClient && dataBuffer != null) { + // Reset the image buffer limit + dataBuffer.limit(dataBuffer.capacity() - 1); + m_camera.getImageData(dataBuffer); + setImageData(new RawData(dataBuffer), 0); + } else { + m_camera.getImage(frame); + setImage(frame); + } + } catch (VisionException ex) { + DriverStation.reportError("Error when getting image from the camera: " + ex.getMessage(), true); + if (dataBuffer != null) { + synchronized (this) { + m_imageDataPool.addLast(dataBuffer); + Timer.delay(.1); + } + } + } + } + } - /** - * check if auto capture is started - * - */ - public synchronized boolean isAutoCaptureStarted() { - return m_autoCaptureStarted; - } - /** - * Set the quality of the compressed image sent to the dashboard - * - * @param quality - * The quality of the JPEG image, from 0 to 100 - */ - public synchronized void setQuality(int quality) { - m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality; - } - /** - * Get the quality of the compressed image sent to the dashboard - * - * @return The quality, from 0 to 100 - */ - public synchronized int getQuality() { - return m_quality; - } + /** + * check if auto capture is started + * + */ + public synchronized boolean isAutoCaptureStarted() { + return m_autoCaptureStarted; + } - /** - * Run the M-JPEG server. - * - * This function listens for a connection from the dashboard in a background - * thread, then sends back the M-JPEG stream. - * - * @throws IOException if the Socket connection fails - * @throws InterruptedException if the sleep is interrupted - */ - protected void serve() throws IOException, InterruptedException { + /** + * Sets the size of the image to use. Use the public kSize constants + * to set the correct mode, or set it directory on a camera and call + * the appropriate autoCapture method + * @param size The size to use + */ + public synchronized void setSize(int size) { + if (m_camera == null) return; + switch (size) { + case kSize640x480: + m_camera.setSize(640, 480); + break; + case kSize320x240: + m_camera.setSize(320, 240); + break; + case kSize160x120: + m_camera.setSize(160, 120); + break; + } + } - ServerSocket socket = new ServerSocket(); - socket.setReuseAddress(true); - InetSocketAddress address = new InetSocketAddress(kPort); - socket.bind(address); + /** + * Set the quality of the compressed image sent to the dashboard + * + * @param quality + * The quality of the JPEG image, from 0 to 100 + */ + public synchronized void setQuality(int quality) { + m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality; + } - while (true) { - try { - Socket s = socket.accept(); + /** + * Get the quality of the compressed image sent to the dashboard + * + * @return The quality, from 0 to 100 + */ + public synchronized int getQuality() { + return m_quality; + } - DataInputStream is = new DataInputStream(s.getInputStream()); - OutputStream os = s.getOutputStream(); + /** + * Run the M-JPEG server. + * + * This function listens for a connection from the dashboard in a background + * thread, then sends back the M-JPEG stream. + * + * @throws IOException if the Socket connection fails + * @throws InterruptedException if the sleep is interrupted + */ + protected void serve() throws IOException, InterruptedException { - int fps = is.readInt(); - int compression = is.readInt(); - int size = is.readInt(); + ServerSocket socket = new ServerSocket(); + socket.setReuseAddress(true); + InetSocketAddress address = new InetSocketAddress(kPort); + socket.bind(address); - if (compression != kHardwareCompression) { - // print to driverstation --> - // ("Choose \"USB Camera HW\" on the dashboard"); - s.close(); - continue; - } + while (true) { + try { + Socket s = socket.accept(); - long period = (long) (1000 / (1.0 * fps)); - while (true) { + DataInputStream is = new DataInputStream(s.getInputStream()); + DataOutputStream os = new DataOutputStream(s.getOutputStream()); - long t0 = System.currentTimeMillis(); + int fps = is.readInt(); + int compression = is.readInt(); + int size = is.readInt(); - while (!ready.get()) - ; - ready.set(false); + if (compression != kHardwareCompression) { + DriverStation.reportError("Choose \"USB Camera HW\" on the dashboard", false); + s.close(); + continue; + } - // write numbers - try { + // Wait for the camera + synchronized (this) { + System.out.println("Camera not yet ready, awaiting image"); + if (m_camera == null) wait(); + m_hwClient = compression == kHardwareCompression; + if (!m_hwClient) setQuality(100 - compression); + else if (m_camera != null) m_camera.setFPS(fps); + setSize(size); + } - os.write(kMagicNumber); + long period = (long) (1000 / (1.0 * fps)); + while (true) { + long t0 = System.currentTimeMillis(); + CameraData imageData = null; + synchronized (this) { + wait(); + imageData = m_imageData; + m_imageData = null; + } - // write size of image + if (imageData == null) continue; + // Set the buffer position to the start of the data, + // and then create a new wrapper for the data at + // exactly that position + imageData.data.getBuffer().position(imageData.start); + byte[] imageArray = new byte[imageData.data.getBuffer().remaining()]; + imageData.data.getBuffer().get(imageArray, 0, imageData.data.getBuffer().remaining()); - synchronized (this) { - os.write(intTobyteArray(m_imageData.size())); + // write numbers + try { + os.write(kMagicNumber); + os.writeInt(imageArray.length); + os.write(imageArray); + os.flush(); + long dt = System.currentTimeMillis() - t0; - byte[] imageData = new byte[m_imageData.size()]; - - for (int i = 0; i < m_imageData.size(); i++) { - imageData[i] = m_imageData.get(i).byteValue(); - } - - os.write(imageData); - } - long dt = System.currentTimeMillis() - t0; - - if (dt < period) { - Thread.sleep(period - dt); - } - - } catch (IOException ex) { - // print error to driverstation - break; - } - } - - } catch (IOException ex) { - // print error to driverstation - continue; - } - } - } - - public byte[] intTobyteArray(int n) { - byte[] arr = new byte[4]; - - arr[0] = (byte) ((n >> 24) & 0xFF); - arr[1] = (byte) ((n >> 16) & 0xFF); - arr[2] = (byte) ((n >> 8) & 0xFF); - arr[3] = (byte) (n & 0xFF); - - return arr; - } + if (dt < period) { + Thread.sleep(period - dt); + } + } catch (IOException | UnsupportedOperationException ex) { + DriverStation.reportError(ex.getMessage(), true); + break; + } finally { + imageData.data.free(); + if (imageData.data.getBuffer() != null) { + synchronized (this) { + m_imageDataPool.addLast(imageData.data.getBuffer()); + } + } + } + } + } catch (IOException ex) { + DriverStation.reportError(ex.getMessage(), true); + continue; + } + } + } } diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/vision/USBCamera.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/vision/USBCamera.java index 8fea4fc953..6f5d606f79 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/vision/USBCamera.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/vision/USBCamera.java @@ -55,6 +55,7 @@ public class USBCamera { } public synchronized void openCamera() { + if (m_id != -1) return; // Camera is already open for (int i=0; i<3; i++) { try { m_id = NIVision.IMAQdxOpenCamera(m_name, @@ -119,7 +120,7 @@ public class USBCamera { if (fps > foundFps) continue; String format = m.group("format"); - boolean isJpeg = format == "jpeg" || format == "JPEG"; + boolean isJpeg = format.equals("jpeg") || format.equals("JPEG"); if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg)) continue; foundMode = mode; @@ -261,33 +262,38 @@ public class USBCamera { } NIVision.IMAQdxGetImageData(m_id, data, NIVision.IMAQdxBufferNumberMode.BufferNumberModeLast, 0); + data.limit(data.capacity() - 1); data.limit(getJpegSize(data)); } private static int getJpegSize(ByteBuffer data) { - if (data.get(0) != 0xff || data.get(1) != 0xd8) + if (data.get(0) != (byte) 0xff || data.get(1) != (byte) 0xd8) throw new VisionException("invalid image"); int pos = 2; while (true) { - byte b = data.get(pos); - if (b != 0xff) - throw new VisionException("invalid image at pos " + pos + " (" + data.get(pos) + ")"); - b = data.get(pos+1); - if (b == 0x01 || (b >= 0xd0 && b <= 0xd7)) // various - pos += 2; - else if (b == 0xd9) // EOI - return pos + 2; - else if (b == 0xd8) // SOI - throw new VisionException("invalid image"); - else if (b == 0xda) { // SOS - int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff); - pos += len + 2; - // Find next marker. Skip over escaped and RST markers. - while (data.get(pos) != 0xff || data.get(pos+1) == 0x00 || (data.get(pos+1) >= 0xd0 && data.get(pos+1) <= 0xd7)) - pos += 1; - } else { // various - int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff); - pos += len + 2; + try { + byte b = data.get(pos); + if (b != (byte) 0xff) + throw new VisionException("invalid image at pos " + pos + " (" + data.get(pos) + ")"); + b = data.get(pos+1); + if (b == (byte) 0x01 || (b >= (byte) 0xd0 && b <= (byte) 0xd7)) // various + pos += 2; + else if (b == (byte) 0xd9) // EOI + return pos + 2; + else if (b == (byte) 0xd8) // SOI + throw new VisionException("invalid image"); + else if (b == (byte) 0xda) { // SOS + int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff); + pos += len + 2; + // Find next marker. Skip over escaped and RST markers. + while (data.get(pos) != (byte) 0xff || data.get(pos+1) == (byte) 0x00 || (data.get(pos+1) >= (byte) 0xd0 && data.get(pos+1) <= (byte) 0xd7)) + pos += 1; + } else { // various + int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff); + pos += len + 2; + } + } catch (IndexOutOfBoundsException ex) { + throw new VisionException("invalid image: could not find jpeg end " + ex.getMessage()); } } }