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}