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