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 org.photonvision.targeting.PhotonPipelineResult;
021import org.wpilib.math.geometry.Transform3d;
022import org.wpilib.networktables.BooleanPublisher;
023import org.wpilib.networktables.BooleanSubscriber;
024import org.wpilib.networktables.BooleanTopic;
025import org.wpilib.networktables.DoubleArrayPublisher;
026import org.wpilib.networktables.DoublePublisher;
027import org.wpilib.networktables.IntegerPublisher;
028import org.wpilib.networktables.IntegerSubscriber;
029import org.wpilib.networktables.IntegerTopic;
030import org.wpilib.networktables.NetworkTable;
031import org.wpilib.networktables.ProtobufPublisher;
032import org.wpilib.networktables.PubSubOption;
033import org.wpilib.networktables.StructPublisher;
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 BooleanPublisher enabledPublisher;
061    public BooleanSubscriber enabledSubscriber;
062
063    public DoublePublisher latencyMillisEntry;
064    public DoublePublisher fpsEntry;
065    public BooleanPublisher hasTargetEntry;
066    public DoublePublisher targetPitchEntry;
067    public DoublePublisher targetYawEntry;
068    public DoublePublisher targetAreaEntry;
069    public StructPublisher<Transform3d> targetPoseEntry;
070    public DoublePublisher targetSkewEntry;
071
072    // The raw position of the best target, in pixels.
073    public DoublePublisher bestTargetPosX;
074    public DoublePublisher bestTargetPosY;
075
076    // Heartbeat
077    public IntegerTopic heartbeatTopic;
078    public IntegerPublisher heartbeatPublisher;
079
080    // Camera Calibration
081    public DoubleArrayPublisher cameraIntrinsicsPublisher;
082    public DoubleArrayPublisher cameraDistortionPublisher;
083
084    public void updateEntries() {
085        var rawBytesEntry =
086                subTable
087                        .getRawTopic("rawBytes")
088                        .publish(
089                                PhotonPipelineResult.photonStruct.getTypeString(),
090                                PubSubOption.periodic(0.01),
091                                PubSubOption.SEND_ALL,
092                                PubSubOption.KEEP_DUPLICATES);
093
094        resultPublisher =
095                new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
096        protoResultPublisher =
097                subTable
098                        .getProtobufTopic("result_proto", PhotonPipelineResult.proto)
099                        .publish(PubSubOption.periodic(0.01), PubSubOption.SEND_ALL);
100
101        pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
102        pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
103
104        driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
105        driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
106
107        // Fun little hack to make the request show up
108        driverModeSubscriber.getTopic().publish().setDefault(false);
109
110        fpsLimitPublisher = subTable.getIntegerTopic("fpsLimit").publish();
111        fpsLimitSubscriber = subTable.getIntegerTopic("fpsLimitRequest").subscribe(-1);
112
113        fpsLimitSubscriber.getTopic().publish().setDefault(-1);
114
115        enabledPublisher = subTable.getBooleanTopic("enabled").publish();
116        enabledSubscriber = subTable.getBooleanTopic("enabledRequest").subscribe(true);
117
118        enabledSubscriber.getTopic().publish().setDefault(true);
119
120        latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
121        fpsEntry = subTable.getDoubleTopic("fps").publish();
122        hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
123
124        targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
125        targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
126        targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
127        targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish();
128        targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();
129
130        bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();
131        bestTargetPosY = subTable.getDoubleTopic("targetPixelsY").publish();
132
133        heartbeatTopic = subTable.getIntegerTopic("heartbeat");
134        heartbeatPublisher = heartbeatTopic.publish();
135
136        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
137        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
138    }
139
140    @SuppressWarnings("DuplicatedCode")
141    public void removeEntries() {
142        if (resultPublisher != null) resultPublisher.close();
143        if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
144        if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
145
146        if (driverModePublisher != null) driverModePublisher.close();
147        if (driverModeSubscriber != null) driverModeSubscriber.close();
148
149        if (fpsLimitPublisher != null) fpsLimitPublisher.close();
150        if (fpsLimitSubscriber != null) fpsLimitSubscriber.close();
151
152        if (enabledPublisher != null) enabledPublisher.close();
153        if (enabledSubscriber != null) enabledSubscriber.close();
154
155        if (latencyMillisEntry != null) latencyMillisEntry.close();
156        if (fpsEntry != null) fpsEntry.close();
157        if (hasTargetEntry != null) hasTargetEntry.close();
158        if (targetPitchEntry != null) targetPitchEntry.close();
159        if (targetAreaEntry != null) targetAreaEntry.close();
160        if (targetYawEntry != null) targetYawEntry.close();
161        if (targetPoseEntry != null) targetPoseEntry.close();
162        if (targetSkewEntry != null) targetSkewEntry.close();
163        if (bestTargetPosX != null) bestTargetPosX.close();
164        if (bestTargetPosY != null) bestTargetPosY.close();
165
166        if (heartbeatPublisher != null) heartbeatPublisher.close();
167
168        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
169        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
170    }
171}