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.websocket;
019
020import java.util.ArrayList;
021import java.util.HashMap;
022import org.photonvision.common.dataflow.CVPipelineResultConsumer;
023import org.photonvision.common.dataflow.DataChangeService;
024import org.photonvision.common.dataflow.events.OutgoingUIEvent;
025import org.photonvision.common.logging.LogGroup;
026import org.photonvision.common.logging.Logger;
027import org.photonvision.common.util.SerializationUtils;
028import org.photonvision.vision.pipeline.result.CVPipelineResult;
029import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
030
031public class UIDataPublisher implements CVPipelineResultConsumer {
032    private static final Logger logger = new Logger(UIDataPublisher.class, LogGroup.VisionModule);
033
034    private final String uniqueName;
035    private long lastUIResultUpdateTime = 0;
036
037    public UIDataPublisher(String uniqueName) {
038        this.uniqueName = uniqueName;
039    }
040
041    @Override
042    public void accept(CVPipelineResult result) {
043        long now = System.currentTimeMillis();
044
045        // only update the UI at 10hz
046        if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
047
048        var dataMap = new HashMap<String, Object>();
049        dataMap.put("sequenceID", result.sequenceID);
050        dataMap.put("fps", result.fps);
051        dataMap.put("latency", result.getLatencyMillis());
052        var uiTargets = new ArrayList<HashMap<String, Object>>(result.targets.size());
053
054        // We don't actually need to send targets during calibration and it can take up a lot (up to
055        // 1.2Mbps for 60 snapshots) of target results with no pitch/yaw/etc set
056        if (!(result instanceof CalibrationPipelineResult)) {
057            for (var t : result.targets) {
058                uiTargets.add(t.toHashMap());
059            }
060        }
061
062        dataMap.put("targets", uiTargets);
063        dataMap.put("classNames", result.objectDetectionClassNames);
064
065        // Only send Multitag Results if they are present, similar to 3d pose
066        if (result.multiTagResult.isPresent()) {
067            var multitagData = new HashMap<String, Object>();
068            multitagData.put(
069                    "bestTransform",
070                    SerializationUtils.transformToHashMap(result.multiTagResult.get().estimatedPose.best));
071            multitagData.put(
072                    "bestReprojectionError", result.multiTagResult.get().estimatedPose.bestReprojErr);
073            multitagData.put("fiducialIDsUsed", result.multiTagResult.get().fiducialIDsUsed);
074            dataMap.put("multitagResult", multitagData);
075        }
076
077        var uiMap = new HashMap<String, HashMap<String, Object>>();
078        uiMap.put(uniqueName, dataMap);
079
080        DataChangeService.getInstance()
081                .publishEvent(OutgoingUIEvent.wrappedOf("updatePipelineResult", uiMap));
082        lastUIResultUpdateTime = now;
083    }
084}