001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.common.networktables;
019
020import edu.wpi.first.math.geometry.Transform3d;
021import edu.wpi.first.networktables.BooleanPublisher;
022import edu.wpi.first.networktables.BooleanSubscriber;
023import edu.wpi.first.networktables.BooleanTopic;
024import edu.wpi.first.networktables.DoubleArrayPublisher;
025import edu.wpi.first.networktables.DoublePublisher;
026import edu.wpi.first.networktables.IntegerPublisher;
027import edu.wpi.first.networktables.IntegerSubscriber;
028import edu.wpi.first.networktables.IntegerTopic;
029import edu.wpi.first.networktables.NetworkTable;
030import edu.wpi.first.networktables.ProtobufPublisher;
031import edu.wpi.first.networktables.PubSubOption;
032import edu.wpi.first.networktables.StructPublisher;
033import org.photonvision.targeting.PhotonPipelineResult;
034
035/**
036 * This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
037 * It's split here so the sim and real-camera implementations can share a common implementation of
038 * the naming and registration of the NT content.
039 *
040 * <p>However, we do expect that the actual logic which fills out values in the entries will be
041 * different for sim vs. real camera
042 */
043@SuppressWarnings("doclint")
044public class NTTopicSet {
045    public NetworkTable subTable;
046
047    public PacketPublisher<PhotonPipelineResult> resultPublisher;
048    public ProtobufPublisher<PhotonPipelineResult> protoResultPublisher;
049
050    public IntegerPublisher pipelineIndexPublisher;
051    public IntegerSubscriber pipelineIndexRequestSub;
052
053    public BooleanTopic driverModeEntry;
054    public BooleanPublisher driverModePublisher;
055    public BooleanSubscriber driverModeSubscriber;
056
057    public IntegerPublisher fpsLimitPublisher;
058    public IntegerSubscriber fpsLimitSubscriber;
059
060    public DoublePublisher latencyMillisEntry;
061    public DoublePublisher fpsEntry;
062    public BooleanPublisher hasTargetEntry;
063    public DoublePublisher targetPitchEntry;
064    public DoublePublisher targetYawEntry;
065    public DoublePublisher targetAreaEntry;
066    public StructPublisher<Transform3d> targetPoseEntry;
067    public DoublePublisher targetSkewEntry;
068
069    // The raw position of the best target, in pixels.
070    public DoublePublisher bestTargetPosX;
071    public DoublePublisher bestTargetPosY;
072
073    // Heartbeat
074    public IntegerTopic heartbeatTopic;
075    public IntegerPublisher heartbeatPublisher;
076
077    // Camera Calibration
078    public DoubleArrayPublisher cameraIntrinsicsPublisher;
079    public DoubleArrayPublisher cameraDistortionPublisher;
080
081    public void updateEntries() {
082        var rawBytesEntry =
083                subTable
084                        .getRawTopic("rawBytes")
085                        .publish(
086                                PhotonPipelineResult.photonStruct.getTypeString(),
087                                PubSubOption.periodic(0.01),
088                                PubSubOption.sendAll(true),
089                                PubSubOption.keepDuplicates(true));
090
091        resultPublisher =
092                new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
093        protoResultPublisher =
094                subTable
095                        .getProtobufTopic("result_proto", PhotonPipelineResult.proto)
096                        .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
097
098        pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
099        pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
100
101        driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
102        driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
103
104        // Fun little hack to make the request show up
105        driverModeSubscriber.getTopic().publish().setDefault(false);
106
107        fpsLimitPublisher = subTable.getIntegerTopic("fpsLimit").publish();
108        fpsLimitSubscriber = subTable.getIntegerTopic("fpsLimitRequest").subscribe(-1);
109
110        fpsLimitSubscriber.getTopic().publish().setDefault(-1);
111
112        latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
113        fpsEntry = subTable.getDoubleTopic("fps").publish();
114        hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
115
116        targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
117        targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
118        targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
119        targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish();
120        targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();
121
122        bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();
123        bestTargetPosY = subTable.getDoubleTopic("targetPixelsY").publish();
124
125        heartbeatTopic = subTable.getIntegerTopic("heartbeat");
126        heartbeatPublisher = heartbeatTopic.publish();
127
128        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
129        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
130    }
131
132    @SuppressWarnings("DuplicatedCode")
133    public void removeEntries() {
134        if (resultPublisher != null) resultPublisher.close();
135        if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
136        if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
137
138        if (driverModePublisher != null) driverModePublisher.close();
139        if (driverModeSubscriber != null) driverModeSubscriber.close();
140
141        if (fpsLimitPublisher != null) fpsLimitPublisher.close();
142        if (fpsLimitSubscriber != null) fpsLimitSubscriber.close();
143
144        if (latencyMillisEntry != null) latencyMillisEntry.close();
145        if (fpsEntry != null) fpsEntry.close();
146        if (hasTargetEntry != null) hasTargetEntry.close();
147        if (targetPitchEntry != null) targetPitchEntry.close();
148        if (targetAreaEntry != null) targetAreaEntry.close();
149        if (targetYawEntry != null) targetYawEntry.close();
150        if (targetPoseEntry != null) targetPoseEntry.close();
151        if (targetSkewEntry != null) targetSkewEntry.close();
152        if (bestTargetPosX != null) bestTargetPosX.close();
153        if (bestTargetPosY != null) bestTargetPosY.close();
154
155        if (heartbeatPublisher != null) heartbeatPublisher.close();
156
157        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
158        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
159    }
160}