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.dataflow.networktables;
019
020import edu.wpi.first.math.geometry.Transform3d;
021import edu.wpi.first.networktables.NetworkTable;
022import edu.wpi.first.networktables.NetworkTableEvent;
023import edu.wpi.first.networktables.NetworkTablesJNI;
024import java.util.List;
025import java.util.function.BooleanSupplier;
026import java.util.function.Consumer;
027import java.util.function.Supplier;
028import org.photonvision.common.configuration.ConfigManager;
029import org.photonvision.common.dataflow.CVPipelineResultConsumer;
030import org.photonvision.common.logging.LogGroup;
031import org.photonvision.common.logging.Logger;
032import org.photonvision.common.networktables.NTTopicSet;
033import org.photonvision.common.util.math.MathUtils;
034import org.photonvision.targeting.PhotonPipelineResult;
035import org.photonvision.vision.pipeline.result.CVPipelineResult;
036import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
037import org.photonvision.vision.target.TrackedTarget;
038
039public class NTDataPublisher implements CVPipelineResultConsumer {
040    private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General);
041
042    private final NetworkTable rootTable = NetworkTablesManager.getInstance().kRootTable;
043
044    private final NTTopicSet ts = new NTTopicSet();
045
046    NTDataChangeListener pipelineIndexListener;
047    private final Supplier<Integer> pipelineIndexSupplier;
048    private final Consumer<Integer> pipelineIndexConsumer;
049
050    NTDataChangeListener driverModeListener;
051    private final BooleanSupplier driverModeSupplier;
052    private final Consumer<Boolean> driverModeConsumer;
053
054    public NTDataPublisher(
055            String cameraNickname,
056            Supplier<Integer> pipelineIndexSupplier,
057            Consumer<Integer> pipelineIndexConsumer,
058            BooleanSupplier driverModeSupplier,
059            Consumer<Boolean> driverModeConsumer) {
060        this.pipelineIndexSupplier = pipelineIndexSupplier;
061        this.pipelineIndexConsumer = pipelineIndexConsumer;
062        this.driverModeSupplier = driverModeSupplier;
063        this.driverModeConsumer = driverModeConsumer;
064
065        updateCameraNickname(cameraNickname);
066        updateEntries();
067    }
068
069    private void onPipelineIndexChange(NetworkTableEvent entryNotification) {
070        var newIndex = (int) entryNotification.valueData.value.getInteger();
071        var originalIndex = pipelineIndexSupplier.get();
072
073        // ignore indexes below 0
074        if (newIndex < 0) {
075            ts.pipelineIndexPublisher.set(originalIndex);
076            return;
077        }
078
079        if (newIndex == originalIndex) {
080            logger.debug("Pipeline index is already " + newIndex);
081            return;
082        }
083
084        pipelineIndexConsumer.accept(newIndex);
085        var setIndex = pipelineIndexSupplier.get();
086        if (newIndex != setIndex) { // set failed
087            ts.pipelineIndexPublisher.set(setIndex);
088            // TODO: Log
089        }
090        logger.debug("Set pipeline index to " + newIndex);
091    }
092
093    private void onDriverModeChange(NetworkTableEvent entryNotification) {
094        var newDriverMode = entryNotification.valueData.value.getBoolean();
095        var originalDriverMode = driverModeSupplier.getAsBoolean();
096
097        if (newDriverMode == originalDriverMode) {
098            logger.debug("Driver mode is already " + newDriverMode);
099            return;
100        }
101
102        driverModeConsumer.accept(newDriverMode);
103        logger.debug("Set driver mode to " + newDriverMode);
104    }
105
106    private void removeEntries() {
107        if (pipelineIndexListener != null) pipelineIndexListener.remove();
108        if (driverModeListener != null) driverModeListener.remove();
109        ts.removeEntries();
110    }
111
112    private void updateEntries() {
113        if (pipelineIndexListener != null) pipelineIndexListener.remove();
114        if (driverModeListener != null) driverModeListener.remove();
115
116        ts.updateEntries();
117
118        pipelineIndexListener =
119                new NTDataChangeListener(
120                        ts.subTable.getInstance(), ts.pipelineIndexRequestSub, this::onPipelineIndexChange);
121
122        driverModeListener =
123                new NTDataChangeListener(
124                        ts.subTable.getInstance(), ts.driverModeSubscriber, this::onDriverModeChange);
125    }
126
127    public void updateCameraNickname(String newCameraNickname) {
128        removeEntries();
129        ts.subTable = rootTable.getSubTable(newCameraNickname);
130        updateEntries();
131    }
132
133    @Override
134    public void accept(CVPipelineResult result) {
135        CVPipelineResult acceptedResult;
136        if (result
137                instanceof
138                CalibrationPipelineResult) // If the data is from a calibration pipeline, override the list
139            // of targets to be null to prevent the data from being sent and
140            // continue to post blank/zero data to the network tables
141            acceptedResult =
142                    new CVPipelineResult(
143                            result.sequenceID,
144                            result.processingNanos,
145                            result.fps,
146                            List.of(),
147                            result.inputAndOutputFrame);
148        else acceptedResult = result;
149        var now = NetworkTablesJNI.now();
150        var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
151
152        var offset = NetworkTablesManager.getInstance().getOffset();
153
154        // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's
155        // timebase
156        var simplified =
157                new PhotonPipelineResult(
158                        acceptedResult.sequenceID,
159                        captureMicros + offset,
160                        now + offset,
161                        NetworkTablesManager.getInstance().getTimeSinceLastPong(),
162                        TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
163                        acceptedResult.multiTagResult);
164
165        // random guess at size of the array
166        ts.resultPublisher.set(simplified, 1024);
167        if (ConfigManager.getInstance().getConfig().getNetworkConfig().shouldPublishProto) {
168            ts.protoResultPublisher.set(simplified);
169        }
170
171        ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
172        ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
173        ts.latencyMillisEntry.set(acceptedResult.getLatencyMillis());
174        ts.hasTargetEntry.set(acceptedResult.hasTargets());
175
176        if (acceptedResult.hasTargets()) {
177            var bestTarget = acceptedResult.targets.get(0);
178
179            ts.targetPitchEntry.set(bestTarget.getPitch());
180            ts.targetYawEntry.set(bestTarget.getYaw());
181            ts.targetAreaEntry.set(bestTarget.getArea());
182            ts.targetSkewEntry.set(bestTarget.getSkew());
183
184            var pose = bestTarget.getBestCameraToTarget3d();
185            ts.targetPoseEntry.set(pose);
186
187            var targetOffsetPoint = bestTarget.getTargetOffsetPoint();
188            ts.bestTargetPosX.set(targetOffsetPoint.x);
189            ts.bestTargetPosY.set(targetOffsetPoint.y);
190        } else {
191            ts.targetPitchEntry.set(0);
192            ts.targetYawEntry.set(0);
193            ts.targetAreaEntry.set(0);
194            ts.targetSkewEntry.set(0);
195            ts.targetPoseEntry.set(new Transform3d());
196            ts.bestTargetPosX.set(0);
197            ts.bestTargetPosY.set(0);
198        }
199
200        // Something in the result can sometimes be null -- so check probably too many things
201        if (acceptedResult.inputAndOutputFrame != null
202                && acceptedResult.inputAndOutputFrame.frameStaticProperties != null
203                && acceptedResult.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
204            var fsp = acceptedResult.inputAndOutputFrame.frameStaticProperties;
205            ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
206            ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
207        } else {
208            ts.cameraIntrinsicsPublisher.accept(new double[] {});
209            ts.cameraDistortionPublisher.accept(new double[] {});
210        }
211
212        ts.heartbeatPublisher.set(acceptedResult.sequenceID);
213
214        // TODO...nt4... is this needed?
215        rootTable.getInstance().flush();
216    }
217}