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 = 1;
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                    \n>>>                                          \s
199                    >>> This is neither tested nor supported.    \s
200                    >>> You MUST update WPILib, PhotonLib, or both.
201                    >>> Check `./gradlew dependencies` and ensure\s
202                    >>> all mentions of OpenCV match the version \s
203                    >>> that PhotonLib was built for. If you find a
204                    >>> a mismatched version in a dependency, you\s
205                    >>> must take steps to update the version of \s
206                    >>> OpenCV used in that dependency. If you do\s
207                    >>> not control that dependency and an updated\s
208                    >>> version is not available, contact the    \s
209                    >>> developers of that dependency.           \s
210                    >>>                                          \s
211                    >>> Your code will now crash.                \s
212                    >>> We hope your day gets better.            \s
213                    >>>                                          \s
214                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
215                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
216                    """;
217
218            DriverStation.reportWarning(bfw, false);
219            DriverStation.reportError(bfw, false);
220            throw new UnsupportedOperationException(bfw);
221        }
222        if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) {
223            String bfw = """
224
225
226
227
228                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
229                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
230                    >>>                                          \s
231                    >>> You are running an incompatible version  \s
232                    >>> of PhotonVision !                        \s
233                    >>>                                          \s
234                    >>> PhotonLib """
235                    + PhotonVersion.versionString
236                    + " is built for OpenCV "
237                    + PhotonVersion.opencvTargetVersion
238                    + "\n"
239                    + ">>> but you are using OpenCV "
240                    + Core.VERSION
241                    + """
242                    \n>>>                                          \s
243                    >>> This is neither tested nor supported.    \s
244                    >>> You MUST update WPILib, PhotonLib, or both.
245                    >>> Check `./gradlew dependencies` and ensure\s
246                    >>> all mentions of OpenCV match the version \s
247                    >>> that PhotonLib was built for. If you find a
248                    >>> a mismatched version in a dependency, you\s
249                    >>> must take steps to update the version of \s
250                    >>> OpenCV used in that dependency. If you do\s
251                    >>> not control that dependency and an updated\s
252                    >>> version is not available, contact the    \s
253                    >>> developers of that dependency.           \s
254                    >>>                                          \s
255                    >>> Your code will now crash.                \s
256                    >>> We hope your day gets better.            \s
257                    >>>                                          \s
258                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s
259                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s
260                    """;
261            // spotless:on
262
263            DriverStation.reportWarning(bfw, false);
264            DriverStation.reportError(bfw, false);
265            throw new UnsupportedOperationException(bfw);
266        }
267    }
268
269    /**
270     * Constructs a PhotonCamera from the name of the camera.
271     *
272     * @param cameraName The nickname of the camera (found in the PhotonVision UI).
273     */
274    public PhotonCamera(String cameraName) {
275        this(NetworkTableInstance.getDefault(), cameraName);
276    }
277
278    /**
279     * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
280     * Calling this function clears the internal FIFO queue, and multiple calls to
281     * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
282     * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
283     * make sure to call this frequently enough to avoid old results being discarded, too!
284     */
285    public List<PhotonPipelineResult> getAllUnreadResults() {
286        verifyVersion();
287        updateDisconnectAlert();
288
289        // Grab the latest results. We don't care about the timestamps from NT - the metadata header has
290        // this, latency compensated by the Time Sync Client
291        var changes = resultSubscriber.getAllChanges();
292        List<PhotonPipelineResult> ret = new ArrayList<>(changes.size());
293        for (var c : changes) {
294            var result = c.value;
295            checkTimeSyncOrWarn(result);
296            ret.add(result);
297        }
298
299        return ret;
300    }
301
302    /**
303     * Returns the latest pipeline result. This is simply the most recent result Received via NT.
304     * Calling this multiple times will always return the most recent result.
305     *
306     * <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
307     * results, or provide duplicate ones!
308     */
309    @Deprecated(since = "2024", forRemoval = true)
310    public PhotonPipelineResult getLatestResult() {
311        verifyVersion();
312        updateDisconnectAlert();
313
314        // Grab the latest result. We don't care about the timestamp from NT - the metadata header has
315        // this, latency compensated by the Time Sync Client
316        var ret = resultSubscriber.get();
317
318        if (ret.timestamp == 0) return new PhotonPipelineResult();
319
320        var result = ret.value;
321
322        checkTimeSyncOrWarn(result);
323
324        return result;
325    }
326
327    private void updateDisconnectAlert() {
328        disconnectAlert.set(!isConnected());
329    }
330
331    private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
332        if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
333            String warningText =
334                    "PhotonVision coprocessor at path "
335                            + path
336                            + " is not connected to the TimeSyncServer? It's been "
337                            + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
338                            + "s since the coprocessor last heard a pong.";
339
340            timesyncAlert.setText(warningText);
341            timesyncAlert.set(true);
342
343            if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
344                prevTimeSyncWarnTime = Timer.getFPGATimestamp();
345
346                DriverStation.reportWarning(
347                        warningText
348                                + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
349                        false);
350            }
351        } else {
352            // Got a valid packet, reset the last time
353            prevTimeSyncWarnTime = 0;
354            timesyncAlert.set(false);
355        }
356    }
357
358    /**
359     * Returns whether the camera is in driver mode.
360     *
361     * @return Whether the camera is in driver mode.
362     */
363    public boolean getDriverMode() {
364        return driverModeSubscriber.get();
365    }
366
367    /**
368     * Toggles driver mode.
369     *
370     * @param driverMode Whether to set driver mode.
371     */
372    public void setDriverMode(boolean driverMode) {
373        driverModePublisher.set(driverMode);
374    }
375
376    /**
377     * Request the camera to save a new image file from the input camera stream with overlays. Images
378     * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
379     * space and eventually cause the system to stop working. Clear out images in
380     * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
381     */
382    public void takeInputSnapshot() {
383        inputSaveImgEntry.set(inputSaveImgEntry.get() + 1);
384    }
385
386    /**
387     * Request the camera to save a new image file from the output stream with overlays. Images take
388     * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
389     * and eventually cause the system to stop working. Clear out images in
390     * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
391     */
392    public void takeOutputSnapshot() {
393        outputSaveImgEntry.set(outputSaveImgEntry.get() + 1);
394    }
395
396    /**
397     * Returns the active pipeline index.
398     *
399     * @return The active pipeline index.
400     */
401    public int getPipelineIndex() {
402        return (int) pipelineIndexState.get(0);
403    }
404
405    /**
406     * Allows the user to select the active pipeline index.
407     *
408     * @param index The active pipeline index.
409     */
410    public void setPipelineIndex(int index) {
411        pipelineIndexRequest.set(index);
412    }
413
414    /**
415     * Returns the current LED mode.
416     *
417     * @return The current LED mode.
418     */
419    public VisionLEDMode getLEDMode() {
420        int value = (int) ledModeState.get(-1);
421        return switch (value) {
422            case 0 -> VisionLEDMode.kOff;
423            case 1 -> VisionLEDMode.kOn;
424            case 2 -> VisionLEDMode.kBlink;
425            default -> VisionLEDMode.kDefault;
426        };
427    }
428
429    /**
430     * Sets the LED mode.
431     *
432     * @param led The mode to set to.
433     */
434    public void setLED(VisionLEDMode led) {
435        ledModeRequest.set(led.value);
436    }
437
438    /**
439     * Returns the name of the camera. This will return the same value that was given to the
440     * constructor as cameraName.
441     *
442     * @return The name of the camera.
443     */
444    public String getName() {
445        return name;
446    }
447
448    /**
449     * Returns whether the camera is connected and actively returning new data. Connection status is
450     * debounced.
451     *
452     * @return True if the camera is actively sending frame data, false otherwise.
453     */
454    public boolean isConnected() {
455        var curHeartbeat = heartbeatSubscriber.get();
456        var now = Timer.getFPGATimestamp();
457
458        if (curHeartbeat < 0) {
459            // we have never heard from the camera
460            return false;
461        }
462
463        if (curHeartbeat != prevHeartbeatValue) {
464            // New heartbeat value from the coprocessor
465            prevHeartbeatChangeTime = now;
466            prevHeartbeatValue = curHeartbeat;
467        }
468
469        return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
470    }
471
472    public Optional<Matrix<N3, N3>> getCameraMatrix() {
473        var cameraMatrix = cameraIntrinsicsSubscriber.get();
474        if (cameraMatrix != null && cameraMatrix.length == 9) {
475            return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix));
476        } else return Optional.empty();
477    }
478
479    /**
480     * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set
481     * to 0
482     */
483    public Optional<Matrix<N8, N1>> getDistCoeffs() {
484        var distCoeffs = cameraDistortionSubscriber.get();
485        if (distCoeffs != null && distCoeffs.length <= 8) {
486            // Copy into array of length 8, and explicitly null higher order terms out
487            double[] data = new double[8];
488            Arrays.fill(data, 0);
489            System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length);
490
491            return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data));
492        } else return Optional.empty();
493    }
494
495    /**
496     * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call
497     * this.
498     */
499    public final NetworkTable getCameraTable() {
500        return cameraTable;
501    }
502
503    void verifyVersion() {
504        if (!VERSION_CHECK_ENABLED) return;
505
506        if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
507        lastVersionCheckTime = Timer.getFPGATimestamp();
508
509        // Heartbeat entry is assumed to always be present. If it's not present, we
510        // assume that a camera with that name was never connected in the first place.
511        if (!heartbeatSubscriber.exists()) {
512            var cameraNames = getTablesThatLookLikePhotonCameras();
513            if (cameraNames.isEmpty()) {
514                DriverStation.reportError(
515                        "Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
516                        false);
517            } else {
518                DriverStation.reportError(
519                        "PhotonVision coprocessor at path "
520                                + path
521                                + " not found on NetworkTables. Double check that your camera names match!",
522                        true);
523
524                var cameraNameStr = new StringBuilder();
525                for (var c : cameraNames) {
526                    cameraNameStr.append(" ==> ");
527                    cameraNameStr.append(c);
528                    cameraNameStr.append("\n");
529                }
530
531                DriverStation.reportError(
532                        "Found the following PhotonVision cameras on NetworkTables:\n"
533                                + cameraNameStr.toString(),
534                        false);
535            }
536        }
537        // Check for connection status. Warn if disconnected.
538        else if (!isConnected()) {
539            DriverStation.reportWarning(
540                    "PhotonVision coprocessor at path " + path + " is not sending new data.", false);
541        }
542
543        String versionString = versionEntry.get("");
544
545        // Check mdef UUID
546        String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID();
547        String remote_uuid = resultSubscriber.getInterfaceUUID();
548
549        if (remote_uuid == null || remote_uuid.isEmpty()) {
550            // not connected yet?
551            DriverStation.reportWarning(
552                    "PhotonVision coprocessor at path "
553                            + path
554                            + " has not reported a message interface UUID - is your coprocessor's camera started?",
555                    false);
556        } else if (!local_uuid.equals(remote_uuid)) {
557            // Error on a verified version mismatch
558            // But stay silent otherwise
559
560            // spotless:off
561            String bfw = """
562
563
564
565
566
567                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
568                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
569                    >>>                                         \s
570                    >>> You are running an incompatible version \s
571                    >>> of PhotonVision on your coprocessor!    \s
572                    >>>                                         \s
573                    >>> This is neither tested nor supported.   \s
574                    >>> You MUST update PhotonVision,           \s
575                    >>> PhotonLib, or both.                     \s
576                    >>>                                         \s
577                    >>> Your code will now crash.               \s
578                    >>> We hope your day gets better.           \s
579                    >>>                                         \s
580                    >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
581                    >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
582                    """;
583            // spotless:on
584
585            DriverStation.reportWarning(bfw, false);
586            var versionMismatchMessage =
587                    "Photon version "
588                            + PhotonVersion.versionString
589                            + " (message definition version "
590                            + local_uuid
591                            + ")"
592                            + " does not match coprocessor version "
593                            + versionString
594                            + " (message definition version "
595                            + remote_uuid
596                            + ")"
597                            + "!";
598            DriverStation.reportError(versionMismatchMessage, false);
599            throw new UnsupportedOperationException(versionMismatchMessage);
600        }
601    }
602
603    private List<String> getTablesThatLookLikePhotonCameras() {
604        return rootPhotonTable.getSubTables().stream()
605                .filter(
606                        it -> {
607                            return rootPhotonTable.getSubTable(it).getEntry("rawBytes").exists();
608                        })
609                .toList();
610    }
611}