From aaa599fa28376c6448e707358d25d9d65c653f36 Mon Sep 17 00:00:00 2001 From: Fredric Silberberg Date: Tue, 13 Jan 2015 22:26:34 -0500 Subject: [PATCH] Updated the Java CameraServer to use the new USBCamera and prevent large camera data copies. Fixed a small bug in USBCamera to prevent reinitializing a camera if it is already initialized. Also fixed some issues with the getJpegSize function to correct for Java incorrectly casting 0xff to an integer, and the byte buffer limit being set incorrectly. Change-Id: I184efd265c617b02523dd9c5d347cc7ca5b4a77b --- .../edu/wpi/first/wpilibj/CameraServer.java | 521 ++++++++++-------- .../wpi/first/wpilibj/vision/USBCamera.java | 48 +- 2 files changed, 330 insertions(+), 239 deletions(-) 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()); } } }