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