001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.cameraserver;
006
007import edu.wpi.first.cscore.CameraServerJNI;
008import edu.wpi.first.cscore.CvSink;
009import edu.wpi.first.cscore.CvSource;
010import edu.wpi.first.cscore.MjpegServer;
011import edu.wpi.first.cscore.UsbCamera;
012import edu.wpi.first.cscore.VideoEvent;
013import edu.wpi.first.cscore.VideoException;
014import edu.wpi.first.cscore.VideoListener;
015import edu.wpi.first.cscore.VideoMode;
016import edu.wpi.first.cscore.VideoSink;
017import edu.wpi.first.cscore.VideoSource;
018import edu.wpi.first.networktables.BooleanEntry;
019import edu.wpi.first.networktables.BooleanPublisher;
020import edu.wpi.first.networktables.IntegerEntry;
021import edu.wpi.first.networktables.IntegerPublisher;
022import edu.wpi.first.networktables.NetworkTable;
023import edu.wpi.first.networktables.NetworkTableInstance;
024import edu.wpi.first.networktables.StringArrayPublisher;
025import edu.wpi.first.networktables.StringArrayTopic;
026import edu.wpi.first.networktables.StringEntry;
027import edu.wpi.first.networktables.StringPublisher;
028import edu.wpi.first.util.PixelFormat;
029import java.lang.ref.Reference;
030import java.util.ArrayList;
031import java.util.Arrays;
032import java.util.HashMap;
033import java.util.Map;
034import java.util.Objects;
035import java.util.concurrent.atomic.AtomicInteger;
036
037/**
038 * Singleton class for creating and keeping camera servers. Also publishes camera information to
039 * NetworkTables.
040 */
041public final class CameraServer {
042  /** CameraServer base port. */
043  public static final int kBasePort = 1181;
044
045  private static final String kPublishName = "/CameraPublisher";
046
047  private static final class PropertyPublisher implements AutoCloseable {
048    @SuppressWarnings({"PMD.MissingBreakInSwitch", "PMD.ImplicitSwitchFallThrough", "fallthrough"})
049    PropertyPublisher(NetworkTable table, VideoEvent event) {
050      String name;
051      String infoName;
052      if (event.name.startsWith("raw_")) {
053        name = "RawProperty/" + event.name;
054        infoName = "RawPropertyInfo/" + event.name;
055      } else {
056        name = "Property/" + event.name;
057        infoName = "PropertyInfo/" + event.name;
058      }
059
060      try {
061        switch (event.propertyKind) {
062          case kBoolean:
063            m_booleanValueEntry = table.getBooleanTopic(name).getEntry(false);
064            m_booleanValueEntry.setDefault(event.value != 0);
065            break;
066          case kEnum:
067            m_choicesTopic = table.getStringArrayTopic(infoName + "/choices");
068            // fall through
069          case kInteger:
070            m_integerValueEntry = table.getIntegerTopic(name).getEntry(0);
071            m_minPublisher = table.getIntegerTopic(infoName + "/min").publish();
072            m_maxPublisher = table.getIntegerTopic(infoName + "/max").publish();
073            m_stepPublisher = table.getIntegerTopic(infoName + "/step").publish();
074            m_defaultPublisher = table.getIntegerTopic(infoName + "/default").publish();
075
076            m_integerValueEntry.setDefault(event.value);
077            m_minPublisher.set(CameraServerJNI.getPropertyMin(event.propertyHandle));
078            m_maxPublisher.set(CameraServerJNI.getPropertyMax(event.propertyHandle));
079            m_stepPublisher.set(CameraServerJNI.getPropertyStep(event.propertyHandle));
080            m_defaultPublisher.set(CameraServerJNI.getPropertyDefault(event.propertyHandle));
081            break;
082          case kString:
083            m_stringValueEntry = table.getStringTopic(name).getEntry("");
084            m_stringValueEntry.setDefault(event.valueStr);
085            break;
086          default:
087            break;
088        }
089      } catch (VideoException ignored) {
090        // ignore
091      }
092    }
093
094    void update(VideoEvent event) {
095      switch (event.propertyKind) {
096        case kBoolean -> {
097          if (m_booleanValueEntry != null) {
098            m_booleanValueEntry.set(event.value != 0);
099          }
100        }
101        case kInteger, kEnum -> {
102          if (m_integerValueEntry != null) {
103            m_integerValueEntry.set(event.value);
104          }
105        }
106        case kString -> {
107          if (m_stringValueEntry != null) {
108            m_stringValueEntry.set(event.valueStr);
109          }
110        }
111        default -> {
112          // NOP
113        }
114      }
115    }
116
117    @Override
118    public void close() {
119      try {
120        if (m_booleanValueEntry != null) {
121          m_booleanValueEntry.close();
122        }
123        if (m_integerValueEntry != null) {
124          m_integerValueEntry.close();
125        }
126        if (m_stringValueEntry != null) {
127          m_stringValueEntry.close();
128        }
129        if (m_minPublisher != null) {
130          m_minPublisher.close();
131        }
132        if (m_maxPublisher != null) {
133          m_maxPublisher.close();
134        }
135        if (m_stepPublisher != null) {
136          m_stepPublisher.close();
137        }
138        if (m_defaultPublisher != null) {
139          m_defaultPublisher.close();
140        }
141        if (m_choicesPublisher != null) {
142          m_choicesPublisher.close();
143        }
144      } finally {
145        Reference.reachabilityFence(m_videoListener);
146      }
147    }
148
149    BooleanEntry m_booleanValueEntry;
150    IntegerEntry m_integerValueEntry;
151    StringEntry m_stringValueEntry;
152    IntegerPublisher m_minPublisher;
153    IntegerPublisher m_maxPublisher;
154    IntegerPublisher m_stepPublisher;
155    IntegerPublisher m_defaultPublisher;
156    StringArrayTopic m_choicesTopic;
157    StringArrayPublisher m_choicesPublisher;
158  }
159
160  private static final class SourcePublisher implements AutoCloseable {
161    SourcePublisher(NetworkTable table, int sourceHandle) {
162      this.m_table = table;
163      m_sourcePublisher = table.getStringTopic("source").publish();
164      m_descriptionPublisher = table.getStringTopic("description").publish();
165      m_connectedPublisher = table.getBooleanTopic("connected").publish();
166      m_streamsPublisher = table.getStringArrayTopic("streams").publish();
167      m_modeEntry = table.getStringTopic("mode").getEntry("");
168      m_modesPublisher = table.getStringArrayTopic("modes").publish();
169
170      m_sourcePublisher.set(makeSourceValue(sourceHandle));
171      m_descriptionPublisher.set(CameraServerJNI.getSourceDescription(sourceHandle));
172      m_connectedPublisher.set(CameraServerJNI.isSourceConnected(sourceHandle));
173      m_streamsPublisher.set(getSourceStreamValues(sourceHandle));
174
175      try {
176        VideoMode mode = CameraServerJNI.getSourceVideoMode(sourceHandle);
177        m_modeEntry.setDefault(videoModeToString(mode));
178        m_modesPublisher.set(getSourceModeValues(sourceHandle));
179      } catch (VideoException ignored) {
180        // Do nothing. Let the other event handlers update this if there is an error.
181      }
182    }
183
184    @Override
185    public void close() throws Exception {
186      m_sourcePublisher.close();
187      m_descriptionPublisher.close();
188      m_connectedPublisher.close();
189      m_streamsPublisher.close();
190      m_modeEntry.close();
191      m_modesPublisher.close();
192      for (PropertyPublisher pp : m_properties.values()) {
193        pp.close();
194      }
195    }
196
197    final NetworkTable m_table;
198    final StringPublisher m_sourcePublisher;
199    final StringPublisher m_descriptionPublisher;
200    final BooleanPublisher m_connectedPublisher;
201    final StringArrayPublisher m_streamsPublisher;
202    final StringEntry m_modeEntry;
203    final StringArrayPublisher m_modesPublisher;
204    final Map<Integer, PropertyPublisher> m_properties = new HashMap<>();
205  }
206
207  private static final AtomicInteger m_defaultUsbDevice = new AtomicInteger();
208  private static String m_primarySourceName;
209  private static final Map<String, VideoSource> m_sources = new HashMap<>();
210  private static final Map<String, VideoSink> m_sinks = new HashMap<>();
211  private static final Map<Integer, SourcePublisher> m_publishers =
212      new HashMap<>(); // indexed by source handle
213  // source handle indexed by sink handle
214  private static final Map<Integer, Integer> m_fixedSources = new HashMap<>();
215  private static final NetworkTable m_publishTable =
216      NetworkTableInstance.getDefault().getTable(kPublishName);
217
218  // We publish sources to NetworkTables using the following structure:
219  // "/CameraPublisher/{Source.Name}/" - root
220  // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
221  // - "streams" (string array): URLs that can be used to stream data
222  // - "description" (string): Description of the source
223  // - "connected" (boolean): Whether source is connected
224  // - "mode" (string): Current video mode
225  // - "modes" (string array): Available video modes
226  // - "Property/{Property}" - Property values
227  // - "PropertyInfo/{Property}" - Property supporting information
228
229  // Listener for video events
230  @SuppressWarnings("PMD.AvoidCatchingGenericException")
231  private static final VideoListener m_videoListener =
232      new VideoListener(
233          event -> {
234            synchronized (CameraServer.class) {
235              switch (event.kind) {
236                case kSourceCreated -> {
237                  // Create subtable for the camera
238                  NetworkTable table = m_publishTable.getSubTable(event.name);
239                  m_publishers.put(
240                      event.sourceHandle, new SourcePublisher(table, event.sourceHandle));
241                }
242                case kSourceDestroyed -> {
243                  SourcePublisher publisher = m_publishers.remove(event.sourceHandle);
244                  if (publisher != null) {
245                    try {
246                      publisher.close();
247                    } catch (Exception e) {
248                      // ignore (nothing we can do about it)
249                    }
250                  }
251                }
252                case kSourceConnected -> {
253                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
254                  if (publisher != null) {
255                    // update the description too (as it may have changed)
256                    publisher.m_descriptionPublisher.set(
257                        CameraServerJNI.getSourceDescription(event.sourceHandle));
258                    publisher.m_connectedPublisher.set(true);
259                  }
260                }
261                case kSourceDisconnected -> {
262                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
263                  if (publisher != null) {
264                    publisher.m_connectedPublisher.set(false);
265                  }
266                }
267                case kSourceVideoModesUpdated -> {
268                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
269                  if (publisher != null) {
270                    publisher.m_modesPublisher.set(getSourceModeValues(event.sourceHandle));
271                  }
272                }
273                case kSourceVideoModeChanged -> {
274                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
275                  if (publisher != null) {
276                    publisher.m_modeEntry.set(videoModeToString(event.mode));
277                  }
278                }
279                case kSourcePropertyCreated -> {
280                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
281                  if (publisher != null) {
282                    publisher.m_properties.put(
283                        event.propertyHandle, new PropertyPublisher(publisher.m_table, event));
284                  }
285                }
286                case kSourcePropertyValueUpdated -> {
287                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
288                  if (publisher != null) {
289                    PropertyPublisher pp = publisher.m_properties.get(event.propertyHandle);
290                    if (pp != null) {
291                      pp.update(event);
292                    }
293                  }
294                }
295                case kSourcePropertyChoicesUpdated -> {
296                  SourcePublisher publisher = m_publishers.get(event.sourceHandle);
297                  if (publisher != null) {
298                    PropertyPublisher pp = publisher.m_properties.get(event.propertyHandle);
299                    if (pp != null && pp.m_choicesTopic != null) {
300                      try {
301                        String[] choices =
302                            CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
303                        if (pp.m_choicesPublisher == null) {
304                          pp.m_choicesPublisher = pp.m_choicesTopic.publish();
305                        }
306                        pp.m_choicesPublisher.set(choices);
307                      } catch (VideoException ignored) {
308                        // ignore (just don't publish choices if we can't get them)
309                      }
310                    }
311                  }
312                }
313                case kSinkSourceChanged,
314                    kSinkCreated,
315                    kSinkDestroyed,
316                    kNetworkInterfacesChanged -> {
317                  m_addresses = CameraServerJNI.getNetworkInterfaces();
318                  updateStreamValues();
319                }
320                default -> {
321                  // NOP
322                }
323              }
324            }
325          },
326          0x4fff,
327          true);
328
329  private static int m_nextPort = kBasePort;
330  private static String[] m_addresses = new String[0];
331
332  /**
333   * Return URI of source with the given index.
334   *
335   * @param source Source index.
336   */
337  private static String makeSourceValue(int source) {
338    return switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
339      case kUsb -> "usb:" + CameraServerJNI.getUsbCameraPath(source);
340      case kHttp -> {
341        String[] urls = CameraServerJNI.getHttpCameraUrls(source);
342        if (urls.length > 0) {
343          yield "ip:" + urls[0];
344        } else {
345          yield "ip:";
346        }
347      }
348      case kCv -> "cv:";
349      case kRaw -> "raw:";
350      case kUnknown -> "unknown:";
351    };
352  }
353
354  /**
355   * Return URI of stream with the given address and port.
356   *
357   * @param address Stream IP address.
358   * @param port Stream remote port.
359   */
360  private static String makeStreamValue(String address, int port) {
361    return "mjpg:http://" + address + ":" + port + "/?action=stream";
362  }
363
364  /**
365   * Return URI of sink stream with the given index.
366   *
367   * @param sink Sink index.
368   */
369  private static synchronized String[] getSinkStreamValues(int sink) {
370    // Ignore all but MjpegServer
371    if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
372      return new String[0];
373    }
374
375    // Get port
376    int port = CameraServerJNI.getMjpegServerPort(sink);
377
378    // Generate values
379    ArrayList<String> values = new ArrayList<>(m_addresses.length + 1);
380    String listenAddress = CameraServerJNI.getMjpegServerListenAddress(sink);
381    if (!listenAddress.isEmpty()) {
382      // If a listen address is specified, only use that
383      values.add(makeStreamValue(listenAddress, port));
384    } else {
385      // Otherwise generate for hostname and all interface addresses
386      values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
387      for (String addr : m_addresses) {
388        if ("127.0.0.1".equals(addr)) {
389          continue; // ignore localhost
390        }
391        values.add(makeStreamValue(addr, port));
392      }
393    }
394
395    return values.toArray(new String[0]);
396  }
397
398  /**
399   * Return list of stream source URIs for the given source index.
400   *
401   * @param source Source index.
402   */
403  private static synchronized String[] getSourceStreamValues(int source) {
404    // Ignore all but HttpCamera
405    if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
406        != VideoSource.Kind.kHttp) {
407      return new String[0];
408    }
409
410    // Generate values
411    String[] values = CameraServerJNI.getHttpCameraUrls(source);
412    for (int j = 0; j < values.length; j++) {
413      values[j] = "mjpg:" + values[j];
414    }
415
416    if (CameraServerSharedStore.getCameraServerShared().isRoboRIO()) {
417      // Look to see if we have a passthrough server for this source
418      // Only do this on the roboRIO
419      for (VideoSink i : m_sinks.values()) {
420        int sink = i.getHandle();
421        int sinkSource = CameraServerJNI.getSinkSource(sink);
422        if (source == sinkSource
423            && VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink))
424                == VideoSink.Kind.kMjpeg) {
425          // Add USB-only passthrough
426          String[] finalValues = Arrays.copyOf(values, values.length + 1);
427          int port = CameraServerJNI.getMjpegServerPort(sink);
428          finalValues[values.length] = makeStreamValue("172.22.11.2", port);
429          return finalValues;
430        }
431      }
432    }
433
434    return values;
435  }
436
437  /** Update list of stream URIs. */
438  private static synchronized void updateStreamValues() {
439    // Over all the sinks...
440    for (VideoSink i : m_sinks.values()) {
441      int sink = i.getHandle();
442
443      // Get the source's subtable (if none exists, we're done)
444      int source =
445          Objects.requireNonNullElseGet(
446              m_fixedSources.get(sink), () -> CameraServerJNI.getSinkSource(sink));
447
448      if (source == 0) {
449        continue;
450      }
451      SourcePublisher publisher = m_publishers.get(source);
452      if (publisher != null) {
453        // Don't set stream values if this is a HttpCamera passthrough
454        if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
455            == VideoSource.Kind.kHttp) {
456          continue;
457        }
458
459        // Set table value
460        String[] values = getSinkStreamValues(sink);
461        if (values.length > 0) {
462          publisher.m_streamsPublisher.set(values);
463        }
464      }
465    }
466
467    // Over all the sources...
468    for (VideoSource i : m_sources.values()) {
469      int source = i.getHandle();
470
471      // Get the source's subtable (if none exists, we're done)
472      SourcePublisher publisher = m_publishers.get(source);
473      if (publisher != null) {
474        // Set table value
475        String[] values = getSourceStreamValues(source);
476        if (values.length > 0) {
477          publisher.m_streamsPublisher.set(values);
478        }
479      }
480    }
481  }
482
483  /** Provide string description of pixel format. */
484  private static String pixelFormatToString(PixelFormat pixelFormat) {
485    return switch (pixelFormat) {
486      case kMJPEG -> "MJPEG";
487      case kYUYV -> "YUYV";
488      case kRGB565 -> "RGB565";
489      case kBGR -> "BGR";
490      case kBGRA -> "BGRA";
491      case kGray -> "Gray";
492      case kY16 -> "Y16";
493      case kUYVY -> "UYVY";
494      case kUnknown -> "Unknown";
495    };
496  }
497
498  /**
499   * Provide string description of video mode.
500   *
501   * <p>The returned string is "{width}x{height} {format} {fps} fps".
502   */
503  private static String videoModeToString(VideoMode mode) {
504    return mode.width
505        + "x"
506        + mode.height
507        + " "
508        + pixelFormatToString(mode.pixelFormat)
509        + " "
510        + mode.fps
511        + " fps";
512  }
513
514  /**
515   * Get list of video modes for the given source handle.
516   *
517   * @param sourceHandle Source handle.
518   */
519  private static String[] getSourceModeValues(int sourceHandle) {
520    VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
521    String[] modeStrings = new String[modes.length];
522    for (int i = 0; i < modes.length; i++) {
523      modeStrings[i] = videoModeToString(modes[i]);
524    }
525    return modeStrings;
526  }
527
528  private CameraServer() {}
529
530  /**
531   * Start automatically capturing images to send to the dashboard.
532   *
533   * <p>You should call this method to see a camera feed on the dashboard. If you also want to
534   * perform vision processing on the roboRIO, use getVideo() to get access to the camera images.
535   *
536   * <p>The first time this overload is called, it calls {@link #startAutomaticCapture(int)} with
537   * device 0, creating a camera named "USB Camera 0". Subsequent calls increment the device number
538   * (e.g. 1, 2, etc).
539   *
540   * @return The USB camera capturing images.
541   */
542  public static UsbCamera startAutomaticCapture() {
543    return startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
544  }
545
546  /**
547   * Start automatically capturing images to send to the dashboard.
548   *
549   * <p>This overload calls {@link #startAutomaticCapture(String, int)} with a name of "USB Camera
550   * {dev}".
551   *
552   * @param dev The device number of the camera interface
553   * @return The USB camera capturing images.
554   */
555  public static UsbCamera startAutomaticCapture(int dev) {
556    UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
557    startAutomaticCapture(camera);
558    CameraServerSharedStore.reportUsage("UsbCamera[" + dev + "]", "auto");
559    return camera;
560  }
561
562  /**
563   * Start automatically capturing images to send to the dashboard.
564   *
565   * @param name The name to give the camera
566   * @param dev The device number of the camera interface
567   * @return The USB camera capturing images.
568   */
569  public static UsbCamera startAutomaticCapture(String name, int dev) {
570    UsbCamera camera = new UsbCamera(name, dev);
571    startAutomaticCapture(camera);
572    CameraServerSharedStore.reportUsage("UsbCamera[" + dev + "]", "name");
573    return camera;
574  }
575
576  /**
577   * Start automatically capturing images to send to the dashboard.
578   *
579   * @param name The name to give the camera
580   * @param path The device path (e.g. "/dev/video0") of the camera
581   * @return The USB camera capturing images.
582   */
583  public static UsbCamera startAutomaticCapture(String name, String path) {
584    UsbCamera camera = new UsbCamera(name, path);
585    startAutomaticCapture(camera);
586    CameraServerSharedStore.reportUsage("UsbCamera[" + path + "]", "path");
587    return camera;
588  }
589
590  /**
591   * Start automatically capturing images to send to the dashboard from an existing camera.
592   *
593   * @param camera Camera
594   * @return The MJPEG server serving images from the given camera.
595   */
596  public static MjpegServer startAutomaticCapture(VideoSource camera) {
597    addCamera(camera);
598    MjpegServer server = addServer("serve_" + camera.getName());
599    server.setSource(camera);
600    return server;
601  }
602
603  /**
604   * Adds a virtual camera for switching between two streams. Unlike the other addCamera methods,
605   * this returns a VideoSink rather than a VideoSource. Calling setSource() on the returned object
606   * can be used to switch the actual source of the stream.
607   *
608   * @param name The name to give the camera
609   * @return The MJPEG server serving images from the given camera.
610   */
611  public static MjpegServer addSwitchedCamera(String name) {
612    // create a dummy CvSource
613    CvSource source = new CvSource(name, PixelFormat.kMJPEG, 160, 120, 30);
614    MjpegServer server = startAutomaticCapture(source);
615    synchronized (CameraServer.class) {
616      m_fixedSources.put(server.getHandle(), source.getHandle());
617    }
618
619    return server;
620  }
621
622  /**
623   * Get OpenCV access to the primary camera feed. This allows you to get images from the camera for
624   * image processing on the roboRIO.
625   *
626   * <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
627   * or addServer().
628   *
629   * @return OpenCV sink for the primary camera feed
630   */
631  public static CvSink getVideo() {
632    VideoSource source;
633    synchronized (CameraServer.class) {
634      if (m_primarySourceName == null) {
635        throw new VideoException("no camera available");
636      }
637      source = m_sources.get(m_primarySourceName);
638    }
639    if (source == null) {
640      throw new VideoException("no camera available");
641    }
642    return getVideo(source);
643  }
644
645  /**
646   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
647   * image processing on the roboRIO.
648   *
649   * @param camera Camera (e.g. as returned by startAutomaticCapture).
650   * @return OpenCV sink for the specified camera
651   */
652  public static CvSink getVideo(VideoSource camera) {
653    String name = "opencv_" + camera.getName();
654
655    synchronized (CameraServer.class) {
656      VideoSink sink = m_sinks.get(name);
657      if (sink != null) {
658        VideoSink.Kind kind = sink.getKind();
659        if (kind != VideoSink.Kind.kCv) {
660          throw new VideoException("expected OpenCV sink, but got " + kind);
661        }
662        return (CvSink) sink;
663      }
664    }
665
666    CvSink newsink = new CvSink(name);
667    newsink.setSource(camera);
668    addServer(newsink);
669    return newsink;
670  }
671
672  /**
673   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
674   * image processing on the roboRIO.
675   *
676   * @param camera Camera (e.g. as returned by startAutomaticCapture).
677   * @param pixelFormat Desired pixelFormat of the camera
678   * @return OpenCV sink for the specified camera
679   */
680  public static CvSink getVideo(VideoSource camera, PixelFormat pixelFormat) {
681    String name = "opencv_" + camera.getName();
682
683    synchronized (CameraServer.class) {
684      VideoSink sink = m_sinks.get(name);
685      if (sink != null) {
686        VideoSink.Kind kind = sink.getKind();
687        if (kind != VideoSink.Kind.kCv) {
688          throw new VideoException("expected OpenCV sink, but got " + kind);
689        }
690        return (CvSink) sink;
691      }
692    }
693
694    CvSink newsink = new CvSink(name, pixelFormat);
695    newsink.setSource(camera);
696    addServer(newsink);
697    return newsink;
698  }
699
700  /**
701   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
702   * image processing on the roboRIO.
703   *
704   * @param name Camera name
705   * @return OpenCV sink for the specified camera
706   */
707  public static CvSink getVideo(String name) {
708    VideoSource source;
709    synchronized (CameraServer.class) {
710      source = m_sources.get(name);
711      if (source == null) {
712        throw new VideoException("could not find camera " + name);
713      }
714    }
715    return getVideo(source);
716  }
717
718  /**
719   * Create a MJPEG stream with OpenCV input. This can be called to pass custom annotated images to
720   * the dashboard.
721   *
722   * @param name Name to give the stream
723   * @param width Width of the image being sent
724   * @param height Height of the image being sent
725   * @return OpenCV source for the MJPEG stream
726   */
727  public static CvSource putVideo(String name, int width, int height) {
728    CvSource source = new CvSource(name, PixelFormat.kMJPEG, width, height, 30);
729    startAutomaticCapture(source);
730    return source;
731  }
732
733  /**
734   * Adds a MJPEG server at the next available port.
735   *
736   * @param name Server name
737   * @return The MJPEG server
738   */
739  public static MjpegServer addServer(String name) {
740    int port;
741    synchronized (CameraServer.class) {
742      port = m_nextPort;
743      m_nextPort++;
744    }
745    return addServer(name, port);
746  }
747
748  /**
749   * Adds a MJPEG server.
750   *
751   * @param name Server name
752   * @param port Server port
753   * @return The MJPEG server
754   */
755  public static MjpegServer addServer(String name, int port) {
756    MjpegServer server = new MjpegServer(name, port);
757    addServer(server);
758    return server;
759  }
760
761  /**
762   * Adds an already created server.
763   *
764   * @param server Server
765   */
766  public static void addServer(VideoSink server) {
767    synchronized (CameraServer.class) {
768      m_sinks.put(server.getName(), server);
769    }
770  }
771
772  /**
773   * Removes a server by name.
774   *
775   * @param name Server name
776   */
777  public static void removeServer(String name) {
778    synchronized (CameraServer.class) {
779      m_sinks.remove(name);
780    }
781  }
782
783  /**
784   * Get server for the primary camera feed.
785   *
786   * <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
787   * or addServer().
788   *
789   * @return The server for the primary camera feed
790   */
791  public static VideoSink getServer() {
792    synchronized (CameraServer.class) {
793      if (m_primarySourceName == null) {
794        throw new VideoException("no camera available");
795      }
796      return getServer("serve_" + m_primarySourceName);
797    }
798  }
799
800  /**
801   * Gets a server by name.
802   *
803   * @param name Server name
804   * @return The server
805   */
806  public static VideoSink getServer(String name) {
807    synchronized (CameraServer.class) {
808      return m_sinks.get(name);
809    }
810  }
811
812  /**
813   * Adds an already created camera.
814   *
815   * @param camera Camera
816   */
817  public static void addCamera(VideoSource camera) {
818    String name = camera.getName();
819    synchronized (CameraServer.class) {
820      if (m_primarySourceName == null) {
821        m_primarySourceName = name;
822      }
823      m_sources.put(name, camera);
824    }
825  }
826
827  /**
828   * Removes a camera by name.
829   *
830   * @param name Camera name
831   */
832  public static void removeCamera(String name) {
833    synchronized (CameraServer.class) {
834      m_sources.remove(name);
835    }
836  }
837}