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 */
043public class NTTopicSet {
044    public NetworkTable subTable;
045
046    public PacketPublisher<PhotonPipelineResult> resultPublisher;
047    public ProtobufPublisher<PhotonPipelineResult> protoResultPublisher;
048
049    public IntegerPublisher pipelineIndexPublisher;
050    public IntegerSubscriber pipelineIndexRequestSub;
051
052    public BooleanTopic driverModeEntry;
053    public BooleanPublisher driverModePublisher;
054    public BooleanSubscriber driverModeSubscriber;
055
056    public DoublePublisher latencyMillisEntry;
057    public DoublePublisher fpsEntry;
058    public BooleanPublisher hasTargetEntry;
059    public DoublePublisher targetPitchEntry;
060    public DoublePublisher targetYawEntry;
061    public DoublePublisher targetAreaEntry;
062    public StructPublisher<Transform3d> targetPoseEntry;
063    public DoublePublisher targetSkewEntry;
064
065    // The raw position of the best target, in pixels.
066    public DoublePublisher bestTargetPosX;
067    public DoublePublisher bestTargetPosY;
068
069    // Heartbeat
070    public IntegerTopic heartbeatTopic;
071    public IntegerPublisher heartbeatPublisher;
072
073    // Camera Calibration
074    public DoubleArrayPublisher cameraIntrinsicsPublisher;
075    public DoubleArrayPublisher cameraDistortionPublisher;
076
077    public void updateEntries() {
078        var rawBytesEntry =
079                subTable
080                        .getRawTopic("rawBytes")
081                        .publish(
082                                PhotonPipelineResult.photonStruct.getTypeString(),
083                                PubSubOption.periodic(0.01),
084                                PubSubOption.sendAll(true),
085                                PubSubOption.keepDuplicates(true));
086
087        resultPublisher =
088                new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
089        protoResultPublisher =
090                subTable
091                        .getProtobufTopic("result_proto", PhotonPipelineResult.proto)
092                        .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
093
094        pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
095        pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
096
097        driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
098        driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
099
100        // Fun little hack to make the request show up
101        driverModeSubscriber.getTopic().publish().setDefault(false);
102
103        latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
104        fpsEntry = subTable.getDoubleTopic("fps").publish();
105        hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
106
107        targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
108        targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
109        targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
110        targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish();
111        targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();
112
113        bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();
114        bestTargetPosY = subTable.getDoubleTopic("targetPixelsY").publish();
115
116        heartbeatTopic = subTable.getIntegerTopic("heartbeat");
117        heartbeatPublisher = heartbeatTopic.publish();
118
119        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
120        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
121    }
122
123    @SuppressWarnings("DuplicatedCode")
124    public void removeEntries() {
125        if (resultPublisher != null) resultPublisher.close();
126        if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
127        if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
128
129        if (driverModePublisher != null) driverModePublisher.close();
130        if (driverModeSubscriber != null) driverModeSubscriber.close();
131
132        if (latencyMillisEntry != null) latencyMillisEntry.close();
133        if (fpsEntry != null) fpsEntry.close();
134        if (hasTargetEntry != null) hasTargetEntry.close();
135        if (targetPitchEntry != null) targetPitchEntry.close();
136        if (targetAreaEntry != null) targetAreaEntry.close();
137        if (targetYawEntry != null) targetYawEntry.close();
138        if (targetPoseEntry != null) targetPoseEntry.close();
139        if (targetSkewEntry != null) targetSkewEntry.close();
140        if (bestTargetPosX != null) bestTargetPosX.close();
141        if (bestTargetPosY != null) bestTargetPosY.close();
142
143        if (heartbeatPublisher != null) heartbeatPublisher.close();
144
145        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
146        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
147    }
148}