wpilibj/src/athena/java/edu/wpi/first/wpilibj/CameraServer.java

   1 /*----------------------------------------------------------------------------*/
   2 /* Copyright (c) FIRST 2016. All Rights Reserved.                             */
   3 /* Open Source Software - may be modified and shared by FRC teams. The code   */
   4 /* must be accompanied by the FIRST BSD license file in the root directory of */
   5 /* the project.                                                               */
   6 /*----------------------------------------------------------------------------*/
   7
   8 package edu.wpi.first.wpilibj;
   9
  10 import java.io.DataInputStream;
  11 import java.io.DataOutputStream;
  12 import java.io.IOException;
  13 import java.io.OutputStream;
  14 import java.net.InetSocketAddress;
  15 import java.net.ServerSocket;
  16 import java.net.Socket;
  17 import java.nio.ByteBuffer;
  18 import java.nio.ByteOrder;
  19 import java.util.ArrayDeque;
  20 import java.util.ArrayList;
  21 import java.util.Deque;
  22 import java.util.List;
  23 import java.util.NoSuchElementException;
  24 import java.util.concurrent.atomic.AtomicBoolean;
  25
  26 import com.ni.vision.NIVision;
  27 import com.ni.vision.NIVision.Image;
  28 import com.ni.vision.NIVision.RawData;
  29 import com.ni.vision.VisionException;
  30
  31 import edu.wpi.first.wpilibj.DriverStation;
  32 import edu.wpi.first.wpilibj.Timer;
  33 import edu.wpi.first.wpilibj.vision.USBCamera;
  34
  35 // replicates CameraServer.cpp in java lib
  36
  37 public class CameraServer {
  38
  39   private static final int kPort = 1180;
  40   private static final byte[] kMagicNumber = {0x01, 0x00, 0x00, 0x00};
  41   private static final int kSize640x480 = 0;
  42   private static final int kSize320x240 = 1;
  43   private static final int kSize160x120 = 2;
  44   private static final int kHardwareCompression = -1;
  45   private static final String kDefaultCameraName = "cam1";
  46   private static final int kMaxImageSize = 200000;
  47   private static CameraServer server;
  48
  49   public static CameraServer getInstance() {
  50     if (server == null) {
  51       server = new CameraServer();
  52     }
  53     return server;
  54   }
  55
  56   private Thread serverThread;
  57   private int m_quality;
  58   private boolean m_autoCaptureStarted;
  59   private boolean m_hwClient = true;
  60   private USBCamera m_camera;
  61   private CameraData m_imageData;
  62   private Deque<ByteBuffer> m_imageDataPool;
  63
  64   private class CameraData {
  65     RawData data;
  66     int start;
  67
  68     public CameraData(RawData d, int s) {
  69       data = d;
  70       start = s;
  71     }
  72   }
  73
  74   private CameraServer() {
  75     m_quality = 50;
  76     m_camera = null;
  77     m_imageData = null;
  78     m_imageDataPool = new ArrayDeque<>(3);
  79     for (int i = 0; i < 3; i++) {
  80       m_imageDataPool.addLast(ByteBuffer.allocateDirect(kMaxImageSize));
  81     }
  82     serverThread = new Thread(new Runnable() {
  83       public void run() {
  84         try {
  85           serve();
  86         } catch (IOException e) {
  87           // do stuff here
  88         } catch (InterruptedException e) {
  89           // do stuff here
  90         }
  91       }
  92     });
  93     serverThread.setName("CameraServer Send Thread");
  94     serverThread.start();
  95   }
  96
  97   private synchronized void setImageData(RawData data, int start) {
  98     if (m_imageData != null && m_imageData.data != null) {
  99       m_imageData.data.free();
 100       if (m_imageData.data.getBuffer() != null)
 101         m_imageDataPool.addLast(m_imageData.data.getBuffer());
 102       m_imageData = null;
 103     }
 104     m_imageData = new CameraData(data, start);
 105     notifyAll();
 106   }
 107
 108   /**
 109    * Manually change the image that is served by the MJPEG stream. This can be
 110    * called to pass custom annotated images to the dashboard. Note that, for
 111    * 640x480 video, this method could take between 40 and 50 milliseconds to
 112    * complete.
 113    *
 114    * This shouldn't be called if {@link #startAutomaticCapture} is called.
 115    *
 116    * @param image The IMAQ image to show on the dashboard
 117    */
 118   public void setImage(Image image) {
 119     // handle multi-threadedness
 120
 121     /* Flatten the IMAQ image to a JPEG */
 122
 123     RawData data =
 124         NIVision.imaqFlatten(image, NIVision.FlattenType.FLATTEN_IMAGE,
 125             NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
 126     ByteBuffer buffer = data.getBuffer();
 127     boolean hwClient;
 128
 129     synchronized (this) {
 130       hwClient = m_hwClient;
 131     }
 132
 133     /* Find the start of the JPEG data */
 134     int index = 0;
 135     if (hwClient) {
 136       while (index < buffer.limit() - 1) {
 137         if ((buffer.get(index) & 0xff) == 0xFF && (buffer.get(index + 1) & 0xff) == 0xD8)
 138           break;
 139         index++;
 140       }
 141     }
 142
 143     if (buffer.limit() - index - 1 <= 2) {
 144       throw new VisionException("data size of flattened image is less than 2. Try another camera! ");
 145     }
 146
 147     setImageData(data, index);
 148   }
 149
 150   /**
 151    * Start automatically capturing images to send to the dashboard. You should
 152    * call this method to just see a camera feed on the dashboard without doing
 153    * any vision processing on the roboRIO. {@link #setImage} shouldn't be called
 154    * after this is called. This overload calles
 155    * {@link #startAutomaticCapture(String)} with the default camera name
 156    */
 157   public void startAutomaticCapture() {
 158     startAutomaticCapture(USBCamera.kDefaultCameraName);
 159   }
 160
 161   /**
 162    * Start automatically capturing images to send to the dashboard.
 163    *
 164    * You should call this method to just see a camera feed on the dashboard
 165    * without doing any vision processing on the roboRIO. {@link #setImage}
 166    * shouldn't be called after this is called.
 167    *
 168    * @param cameraName The name of the camera interface (e.g. "cam1")
 169    */
 170   public void startAutomaticCapture(String cameraName) {
 171     try {
 172       USBCamera camera = new USBCamera(cameraName);
 173       camera.openCamera();
 174       startAutomaticCapture(camera);
 175     } catch (VisionException ex) {
 176       DriverStation.reportError(
 177           "Error when starting the camera: " + cameraName + " " + ex.getMessage(), true);
 178     }
 179   }
 180
 181   public synchronized void startAutomaticCapture(USBCamera camera) {
 182     if (m_autoCaptureStarted)
 183       return;
 184     m_autoCaptureStarted = true;
 185     m_camera = camera;
 186
 187     m_camera.startCapture();
 188
 189     Thread captureThread = new Thread(new Runnable() {
 190       @Override
 191       public void run() {
 192         capture();
 193       }
 194     });
 195     captureThread.setName("Camera Capture Thread");
 196     captureThread.start();
 197   }
 198
 199   protected void capture() {
 200     Image frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
 201     while (true) {
 202       boolean hwClient;
 203       ByteBuffer dataBuffer = null;
 204       synchronized (this) {
 205         hwClient = m_hwClient;
 206         if (hwClient) {
 207           dataBuffer = m_imageDataPool.removeLast();
 208         }
 209       }
 210
 211       try {
 212         if (hwClient && dataBuffer != null) {
 213           // Reset the image buffer limit
 214           dataBuffer.limit(dataBuffer.capacity() - 1);
 215           m_camera.getImageData(dataBuffer);
 216           setImageData(new RawData(dataBuffer), 0);
 217         } else {
 218           m_camera.getImage(frame);
 219           setImage(frame);
 220         }
 221       } catch (VisionException ex) {
 222         DriverStation.reportError("Error when getting image from the camera: " + ex.getMessage(),
 223             true);
 224         if (dataBuffer != null) {
 225           synchronized (this) {
 226             m_imageDataPool.addLast(dataBuffer);
 227             Timer.delay(.1);
 228           }
 229         }
 230       }
 231     }
 232   }
 233
 234
 235
 236   /**
 237    * check if auto capture is started
 238    *
 239    */
 240   public synchronized boolean isAutoCaptureStarted() {
 241     return m_autoCaptureStarted;
 242   }
 243
 244   /**
 245    * Sets the size of the image to use. Use the public kSize constants to set
 246    * the correct mode, or set it directory on a camera and call the appropriate
 247    * autoCapture method
 248    *$
 249    * @param size The size to use
 250    */
 251   public synchronized void setSize(int size) {
 252     if (m_camera == null)
 253       return;
 254     switch (size) {
 255       case kSize640x480:
 256         m_camera.setSize(640, 480);
 257         break;
 258       case kSize320x240:
 259         m_camera.setSize(320, 240);
 260         break;
 261       case kSize160x120:
 262         m_camera.setSize(160, 120);
 263         break;
 264     }
 265   }
 266
 267   /**
 268    * Set the quality of the compressed image sent to the dashboard
 269    *
 270    * @param quality The quality of the JPEG image, from 0 to 100
 271    */
 272   public synchronized void setQuality(int quality) {
 273     m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality;
 274   }
 275
 276   /**
 277    * Get the quality of the compressed image sent to the dashboard
 278    *
 279    * @return The quality, from 0 to 100
 280    */
 281   public synchronized int getQuality() {
 282     return m_quality;
 283   }
 284
 285   /**
 286    * Run the M-JPEG server.
 287    *
 288    * This function listens for a connection from the dashboard in a background
 289    * thread, then sends back the M-JPEG stream.
 290    *
 291    * @throws IOException if the Socket connection fails
 292    * @throws InterruptedException if the sleep is interrupted
 293    */
 294   protected void serve() throws IOException, InterruptedException {
 295
 296     ServerSocket socket = new ServerSocket();
 297     socket.setReuseAddress(true);
 298     InetSocketAddress address = new InetSocketAddress(kPort);
 299     socket.bind(address);
 300
 301     while (true) {
 302       try {
 303         Socket s = socket.accept();
 304
 305         DataInputStream is = new DataInputStream(s.getInputStream());
 306         DataOutputStream os = new DataOutputStream(s.getOutputStream());
 307
 308         int fps = is.readInt();
 309         int compression = is.readInt();
 310         int size = is.readInt();
 311
 312         if (compression != kHardwareCompression) {
 313           DriverStation.reportError("Choose \"USB Camera HW\" on the dashboard", false);
 314           s.close();
 315           continue;
 316         }
 317
 318         // Wait for the camera
 319         synchronized (this) {
 320           System.out.println("Camera not yet ready, awaiting image");
 321           if (m_camera == null)
 322             wait();
 323           m_hwClient = compression == kHardwareCompression;
 324           if (!m_hwClient)
 325             setQuality(100 - compression);
 326           else if (m_camera != null)
 327             m_camera.setFPS(fps);
 328           setSize(size);
 329         }
 330
 331         long period = (long) (1000 / (1.0 * fps));
 332         while (true) {
 333           long t0 = System.currentTimeMillis();
 334           CameraData imageData = null;
 335           synchronized (this) {
 336             wait();
 337             imageData = m_imageData;
 338             m_imageData = null;
 339           }
 340
 341           if (imageData == null)
 342             continue;
 343           // Set the buffer position to the start of the data,
 344           // and then create a new wrapper for the data at
 345           // exactly that position
 346           imageData.data.getBuffer().position(imageData.start);
 347           byte[] imageArray = new byte[imageData.data.getBuffer().remaining()];
 348           imageData.data.getBuffer().get(imageArray, 0, imageData.data.getBuffer().remaining());
 349
 350           // write numbers
 351           try {
 352             os.write(kMagicNumber);
 353             os.writeInt(imageArray.length);
 354             os.write(imageArray);
 355             os.flush();
 356             long dt = System.currentTimeMillis() - t0;
 357
 358             if (dt < period) {
 359               Thread.sleep(period - dt);
 360             }
 361           } catch (IOException | UnsupportedOperationException ex) {
 362             DriverStation.reportError(ex.getMessage(), true);
 363             break;
 364           } finally {
 365             imageData.data.free();
 366             if (imageData.data.getBuffer() != null) {
 367               synchronized (this) {
 368                 m_imageDataPool.addLast(imageData.data.getBuffer());
 369               }
 370             }
 371           }
 372         }
 373       } catch (IOException ex) {
 374         DriverStation.reportError(ex.getMessage(), true);
 375         continue;
 376       }
 377     }
 378   }
 379 }