001/*
002 * MIT License
003 *
004 * Copyright (c) PhotonVision
005 *
006 * Permission is hereby granted, free of charge, to any person obtaining a copy
007 * of this software and associated documentation files (the "Software"), to deal
008 * in the Software without restriction, including without limitation the rights
009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
010 * copies of the Software, and to permit persons to whom the Software is
011 * furnished to do so, subject to the following conditions:
012 *
013 * The above copyright notice and this permission notice shall be included in all
014 * copies or substantial portions of the Software.
015 *
016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
022 * SOFTWARE.
023 */
024
025package org.photonvision;
026
027import edu.wpi.first.hal.FRCNetComm.tResourceType;
028import edu.wpi.first.hal.HAL;
029import edu.wpi.first.math.MatBuilder;
030import edu.wpi.first.math.Matrix;
031import edu.wpi.first.math.Nat;
032import edu.wpi.first.math.numbers.*;
033import edu.wpi.first.networktables.BooleanPublisher;
034import edu.wpi.first.networktables.BooleanSubscriber;
035import edu.wpi.first.networktables.DoubleArraySubscriber;
036import edu.wpi.first.networktables.IntegerEntry;
037import edu.wpi.first.networktables.IntegerPublisher;
038import edu.wpi.first.networktables.IntegerSubscriber;
039import edu.wpi.first.networktables.MultiSubscriber;
040import edu.wpi.first.networktables.NetworkTable;
041import edu.wpi.first.networktables.NetworkTableInstance;
042import edu.wpi.first.networktables.PubSubOption;
043import edu.wpi.first.networktables.StringSubscriber;
044import edu.wpi.first.wpilibj.Alert;
045import edu.wpi.first.wpilibj.Alert.AlertType;
046import edu.wpi.first.wpilibj.DriverStation;
047import edu.wpi.first.wpilibj.Timer;
048import edu.wpi.first.wpilibj.util.WPILibVersion;
049import java.util.ArrayList;
050import java.util.Arrays;
051import java.util.List;
052import java.util.Optional;
053import org.opencv.core.Core;
054import org.photonvision.common.hardware.VisionLEDMode;
055import org.photonvision.common.networktables.PacketSubscriber;
056import org.photonvision.targeting.PhotonPipelineResult;
057import org.photonvision.timesync.TimeSyncSingleton;
058
059/** Represents a camera that is connected to PhotonVision. */
060public class PhotonCamera implements AutoCloseable {
061    private static int InstanceCount = 0;
062    public static final String kTableName = "photonvision";
063    private static final String PHOTON_ALERT_GROUP = "PhotonAlerts";
064
065    private final NetworkTable cameraTable;
066    PacketSubscriber<PhotonPipelineResult> resultSubscriber;
067    BooleanPublisher driverModePublisher;
068    BooleanSubscriber driverModeSubscriber;
069    StringSubscriber versionEntry;
070    IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
071    IntegerPublisher pipelineIndexRequest, ledModeRequest;
072    IntegerSubscriber pipelineIndexState, ledModeState;
073    IntegerSubscriber heartbeatSubscriber;
074    DoubleArraySubscriber cameraIntrinsicsSubscriber;
075    DoubleArraySubscriber cameraDistortionSubscriber;
076    MultiSubscriber topicNameSubscriber;
077    NetworkTable rootPhotonTable;
078
079    @Override
080    public void close() {
081        resultSubscriber.close();
082        driverModePublisher.close();
083        driverModeSubscriber.close();
084        versionEntry.close();
085        inputSaveImgEntry.close();
086        outputSaveImgEntry.close();
087        pipelineIndexRequest.close();
088        pipelineIndexState.close();
089        ledModeRequest.close();
090        ledModeState.close();
091        pipelineIndexRequest.close();
092        cameraIntrinsicsSubscriber.close();
093        cameraDistortionSubscriber.close();
094        topicNameSubscriber.close();
095    }
096
097    private final String path;
098    private final String name;
099
100    private static boolean VERSION_CHECK_ENABLED = true;
101    private static long VERSION_CHECK_INTERVAL = 5;
102    double lastVersionCheckTime = 0;
103
104    private long prevHeartbeatValue = -1;
105    private double prevHeartbeatChangeTime = 0;
106    private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
107
108    double prevTimeSyncWarnTime = 0;
109    private static final double WARN_DEBOUNCE_SEC = 5;
110
111    private final Alert disconnectAlert;
112    private final Alert timesyncAlert;
113
114    public static void setVersionCheckEnabled(boolean enabled) {
115        VERSION_CHECK_ENABLED = enabled;
116    }
117
118    /**
119     * Constructs a PhotonCamera from a root table.
120     *
121     * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in
122     *     simulation, but should *usually* be the default NTInstance from
123     *     NetworkTableInstance::getDefault
124     * @param cameraName The name of the camera, as seen in the UI.
125     */
126    public PhotonCamera(NetworkTableInstance instance, String cameraName) {
127        name = cameraName;
128        disconnectAlert =
129                new Alert(
130                        PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning);
131        timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning);
132        rootPhotonTable = instance.getTable(kTableName);
133        this.cameraTable = rootPhotonTable.getSubTable(cameraName);
134        path = cameraTable.getPath();
135        var rawBytesEntry =
136                cameraTable
137                        .getRawTopic("rawBytes")
138                        .subscribe(
139                                PhotonPipelineResult.photonStruct.getTypeString(),
140                                new byte[0],
141                                PubSubOption.periodic(0.01),
142                                PubSubOption.sendAll(true),
143                                PubSubOption.pollStorage(20));
144        resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
145        driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
146        driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
147        inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
148        outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
149        pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
150        pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0);
151        heartbeatSubscriber = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
152        cameraIntrinsicsSubscriber =
153                cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
154        cameraDistortionSubscriber =
155                cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null);
156
157        ledModeRequest = rootPhotonTable.getIntegerTopic("ledModeRequest").publish();
158        ledModeState = rootPhotonTable.getIntegerTopic("ledModeState").subscribe(-1);
159        versionEntry = rootPhotonTable.getStringTopic("version").subscribe("");
160
161        // Existing is enough to make this multisubscriber do its thing
162        topicNameSubscriber =
163                new MultiSubscriber(
164                        instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
165
166        HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount);
167        InstanceCount++;
168
169        // HACK - start a TimeSyncServer, if we haven't yet.
170        TimeSyncSingleton.load();
171
172        // HACK - check if things are compatible
173        verifyDependencies();
174    }
175
176    public static void verifyDependencies() {
177        // spotless:off
178        if (!WPILibVersion.Version.equals(PhotonVersion.wpilibTargetVersion)) {
179            String bfw = """
180
181
182
183
184                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
185                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
186                    >>>                                          \s
187                    >>> You are running an incompatible version  \s
188                    >>> of PhotonVision !                        \s
189                    >>>                                          \s
190                    >>> PhotonLib """
191                    + PhotonVersion.versionString
192                    + " is built for WPILib "
193                    + PhotonVersion.wpilibTargetVersion
194                    + "\n"
195                    + ">>> but you are using WPILib "
196                    + WPILibVersion.Version
197                    + """
198                    >>>                                          \s
199                    >>> This is neither tested nor supported.    \s
200                    >>> You MUST update PhotonVision,            \s
201                    >>> PhotonLib, or both.                      \s
202                    >>> Verify the output of `./gradlew dependencies`
203                    >>>                                          \s
204                    >>> Your code will now crash.                \s
205                    >>> We hope your day gets better.            \s
206                    >>>                                          \s
207                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
208                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
209                    """;
210
211            DriverStation.reportWarning(bfw, false);
212            DriverStation.reportError(bfw, false);
213            throw new UnsupportedOperationException(bfw);
214        }
215        if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) {
216            String bfw = """
217
218
219
220
221                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
222                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
223                    >>>                                          \s
224                    >>> You are running an incompatible version  \s
225                    >>> of PhotonVision !                        \s
226                    >>>                                          \s
227                    >>> PhotonLib """
228                    + PhotonVersion.versionString
229                    + " is built for OpenCV "
230                    + PhotonVersion.opencvTargetVersion
231                    + "\n"
232                    + ">>> but you are using OpenCV "
233                    + Core.VERSION
234                    + """
235                    >>>                                          \s
236                    >>> This is neither tested nor supported.    \s
237                    >>> You MUST update PhotonVision,            \s
238                    >>> PhotonLib, or both.                      \s
239                    >>> Verify the output of `./gradlew dependencies`
240                    >>>                                          \s
241                    >>> Your code will now crash.                \s
242                    >>> We hope your day gets better.            \s
243                    >>>                                          \s
244                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
245                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
246                    """;
247            // spotless:on
248
249            DriverStation.reportWarning(bfw, false);
250            DriverStation.reportError(bfw, false);
251            throw new UnsupportedOperationException(bfw);
252        }
253    }
254
255    /**
256     * Constructs a PhotonCamera from the name of the camera.
257     *
258     * @param cameraName The nickname of the camera (found in the PhotonVision UI).
259     */
260    public PhotonCamera(String cameraName) {
261        this(NetworkTableInstance.getDefault(), cameraName);
262    }
263
264    /**
265     * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
266     * Calling this function clears the internal FIFO queue, and multiple calls to
267     * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
268     * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
269     * make sure to call this frequently enough to avoid old results being discarded, too!
270     */
271    public List<PhotonPipelineResult> getAllUnreadResults() {
272        verifyVersion();
273        updateDisconnectAlert();
274
275        List<PhotonPipelineResult> ret = new ArrayList<>();
276
277        // Grab the latest results. We don't care about the timestamps from NT - the metadata header has
278        // this, latency compensated by the Time Sync Client
279        var changes = resultSubscriber.getAllChanges();
280        for (var c : changes) {
281            var result = c.value;
282            checkTimeSyncOrWarn(result);
283            ret.add(result);
284        }
285
286        return ret;
287    }
288
289    /**
290     * Returns the latest pipeline result. This is simply the most recent result Received via NT.
291     * Calling this multiple times will always return the most recent result.
292     *
293     * <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
294     * results, or provide duplicate ones!
295     */
296    @Deprecated(since = "2024", forRemoval = true)
297    public PhotonPipelineResult getLatestResult() {
298        verifyVersion();
299        updateDisconnectAlert();
300
301        // Grab the latest result. We don't care about the timestamp from NT - the metadata header has
302        // this, latency compensated by the Time Sync Client
303        var ret = resultSubscriber.get();
304
305        if (ret.timestamp == 0) return new PhotonPipelineResult();
306
307        var result = ret.value;
308
309        checkTimeSyncOrWarn(result);
310
311        return result;
312    }
313
314    private void updateDisconnectAlert() {
315        disconnectAlert.set(!isConnected());
316    }
317
318    private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
319        if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
320            String warningText =
321                    "PhotonVision coprocessor at path "
322                            + path
323                            + " is not connected to the TimeSyncServer? It's been "
324                            + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
325                            + "s since the coprocessor last heard a pong.";
326
327            timesyncAlert.setText(warningText);
328            timesyncAlert.set(true);
329
330            if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
331                prevTimeSyncWarnTime = Timer.getFPGATimestamp();
332
333                DriverStation.reportWarning(
334                        warningText
335                                + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
336                        false);
337            }
338        } else {
339            // Got a valid packet, reset the last time
340            prevTimeSyncWarnTime = 0;
341            timesyncAlert.set(false);
342        }
343    }
344
345    /**
346     * Returns whether the camera is in driver mode.
347     *
348     * @return Whether the camera is in driver mode.
349     */
350    public boolean getDriverMode() {
351        return driverModeSubscriber.get();
352    }
353
354    /**
355     * Toggles driver mode.
356     *
357     * @param driverMode Whether to set driver mode.
358     */
359    public void setDriverMode(boolean driverMode) {
360        driverModePublisher.set(driverMode);
361    }
362
363    /**
364     * Request the camera to save a new image file from the input camera stream with overlays. Images
365     * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
366     * space and eventually cause the system to stop working. Clear out images in
367     * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
368     */
369    public void takeInputSnapshot() {
370        inputSaveImgEntry.set(inputSaveImgEntry.get() + 1);
371    }
372
373    /**
374     * Request the camera to save a new image file from the output stream with overlays. Images take
375     * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
376     * and eventually cause the system to stop working. Clear out images in
377     * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
378     */
379    public void takeOutputSnapshot() {
380        outputSaveImgEntry.set(outputSaveImgEntry.get() + 1);
381    }
382
383    /**
384     * Returns the active pipeline index.
385     *
386     * @return The active pipeline index.
387     */
388    public int getPipelineIndex() {
389        return (int) pipelineIndexState.get(0);
390    }
391
392    /**
393     * Allows the user to select the active pipeline index.
394     *
395     * @param index The active pipeline index.
396     */
397    public void setPipelineIndex(int index) {
398        pipelineIndexRequest.set(index);
399    }
400
401    /**
402     * Returns the current LED mode.
403     *
404     * @return The current LED mode.
405     */
406    public VisionLEDMode getLEDMode() {
407        int value = (int) ledModeState.get(-1);
408        return switch (value) {
409            case 0 -> VisionLEDMode.kOff;
410            case 1 -> VisionLEDMode.kOn;
411            case 2 -> VisionLEDMode.kBlink;
412            default -> VisionLEDMode.kDefault;
413        };
414    }
415
416    /**
417     * Sets the LED mode.
418     *
419     * @param led The mode to set to.
420     */
421    public void setLED(VisionLEDMode led) {
422        ledModeRequest.set(led.value);
423    }
424
425    /**
426     * Returns the name of the camera. This will return the same value that was given to the
427     * constructor as cameraName.
428     *
429     * @return The name of the camera.
430     */
431    public String getName() {
432        return name;
433    }
434
435    /**
436     * Returns whether the camera is connected and actively returning new data. Connection status is
437     * debounced.
438     *
439     * @return True if the camera is actively sending frame data, false otherwise.
440     */
441    public boolean isConnected() {
442        var curHeartbeat = heartbeatSubscriber.get();
443        var now = Timer.getFPGATimestamp();
444
445        if (curHeartbeat < 0) {
446            // we have never heard from the camera
447            return false;
448        }
449
450        if (curHeartbeat != prevHeartbeatValue) {
451            // New heartbeat value from the coprocessor
452            prevHeartbeatChangeTime = now;
453            prevHeartbeatValue = curHeartbeat;
454        }
455
456        return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
457    }
458
459    public Optional<Matrix<N3, N3>> getCameraMatrix() {
460        var cameraMatrix = cameraIntrinsicsSubscriber.get();
461        if (cameraMatrix != null && cameraMatrix.length == 9) {
462            return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix));
463        } else return Optional.empty();
464    }
465
466    /**
467     * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set
468     * to 0
469     */
470    public Optional<Matrix<N8, N1>> getDistCoeffs() {
471        var distCoeffs = cameraDistortionSubscriber.get();
472        if (distCoeffs != null && distCoeffs.length <= 8) {
473            // Copy into array of length 8, and explicitly null higher order terms out
474            double[] data = new double[8];
475            Arrays.fill(data, 0);
476            System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length);
477
478            return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data));
479        } else return Optional.empty();
480    }
481
482    /**
483     * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call
484     * this.
485     */
486    public final NetworkTable getCameraTable() {
487        return cameraTable;
488    }
489
490    void verifyVersion() {
491        if (!VERSION_CHECK_ENABLED) return;
492
493        if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
494        lastVersionCheckTime = Timer.getFPGATimestamp();
495
496        // Heartbeat entry is assumed to always be present. If it's not present, we
497        // assume that a camera with that name was never connected in the first place.
498        if (!heartbeatSubscriber.exists()) {
499            var cameraNames = getTablesThatLookLikePhotonCameras();
500            if (cameraNames.isEmpty()) {
501                DriverStation.reportError(
502                        "Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
503                        false);
504            } else {
505                DriverStation.reportError(
506                        "PhotonVision coprocessor at path "
507                                + path
508                                + " not found on NetworkTables. Double check that your camera names match!",
509                        true);
510
511                var cameraNameStr = new StringBuilder();
512                for (var c : cameraNames) {
513                    cameraNameStr.append(" ==> ");
514                    cameraNameStr.append(c);
515                    cameraNameStr.append("\n");
516                }
517
518                DriverStation.reportError(
519                        "Found the following PhotonVision cameras on NetworkTables:\n"
520                                + cameraNameStr.toString(),
521                        false);
522            }
523        }
524        // Check for connection status. Warn if disconnected.
525        else if (!isConnected()) {
526            DriverStation.reportWarning(
527                    "PhotonVision coprocessor at path " + path + " is not sending new data.", false);
528        }
529
530        String versionString = versionEntry.get("");
531
532        // Check mdef UUID
533        String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID();
534        String remote_uuid = resultSubscriber.getInterfaceUUID();
535
536        if (remote_uuid == null || remote_uuid.isEmpty()) {
537            // not connected yet?
538            DriverStation.reportWarning(
539                    "PhotonVision coprocessor at path "
540                            + path
541                            + " has not reported a message interface UUID - is your coprocessor's camera started?",
542                    false);
543        } else if (!local_uuid.equals(remote_uuid)) {
544            // Error on a verified version mismatch
545            // But stay silent otherwise
546
547            // spotless:off
548            String bfw = """
549
550
551
552
553
554                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
555                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
556                    >>>                                         \s
557                    >>> You are running an incompatible version \s
558                    >>> of PhotonVision on your coprocessor!    \s
559                    >>>                                         \s
560                    >>> This is neither tested nor supported.   \s
561                    >>> You MUST update PhotonVision,           \s
562                    >>> PhotonLib, or both.                     \s
563                    >>>                                         \s
564                    >>> Your code will now crash.               \s
565                    >>> We hope your day gets better.           \s
566                    >>>                                         \s
567                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
568                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
569                    """;
570            // spotless:on
571
572            DriverStation.reportWarning(bfw, false);
573            var versionMismatchMessage =
574                    "Photon version "
575                            + PhotonVersion.versionString
576                            + " (message definition version "
577                            + local_uuid
578                            + ")"
579                            + " does not match coprocessor version "
580                            + versionString
581                            + " (message definition version "
582                            + remote_uuid
583                            + ")"
584                            + "!";
585            DriverStation.reportError(versionMismatchMessage, false);
586            throw new UnsupportedOperationException(versionMismatchMessage);
587        }
588    }
589
590    private List<String> getTablesThatLookLikePhotonCameras() {
591        return rootPhotonTable.getSubTables().stream()
592                .filter(
593                        it -> {
594                            return rootPhotonTable.getSubTable(it).getEntry("rawBytes").exists();
595                        })
596                .toList();
597    }
598}