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.configuration;
019
020import com.fasterxml.jackson.annotation.JsonCreator;
021import com.fasterxml.jackson.annotation.JsonIgnore;
022import com.fasterxml.jackson.annotation.JsonProperty;
023import edu.wpi.first.cscore.UsbCameraInfo;
024import java.util.ArrayList;
025import java.util.List;
026import java.util.UUID;
027import org.photonvision.common.dataflow.websocket.UICameraConfiguration;
028import org.photonvision.common.logging.LogGroup;
029import org.photonvision.common.logging.Logger;
030import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
031import org.photonvision.vision.camera.CameraType;
032import org.photonvision.vision.camera.PVCameraInfo;
033import org.photonvision.vision.camera.QuirkyCamera;
034import org.photonvision.vision.pipeline.CVPipelineSettings;
035import org.photonvision.vision.pipeline.DriverModePipelineSettings;
036import org.photonvision.vision.processes.PipelineManager;
037
038public class CameraConfiguration {
039    private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera);
040
041    /** A unique name (ostensibly an opaque UUID) to identify this particular configuration */
042    public String uniqueName = "";
043
044    /**
045     * The info of the camera we last matched to. We still match by unique path (where we can), but
046     * this is useful to provide warnings to users
047     */
048    public PVCameraInfo matchedCameraInfo;
049
050    /** User-set nickname */
051    public String nickname = "";
052
053    /** Deactivated vision modules do not open camera hardware or lock USB ports */
054    public boolean deactivated = false;
055
056    public QuirkyCamera cameraQuirks;
057
058    public double FOV = 70;
059    public List<CameraCalibrationCoefficients> calibrations = new ArrayList<>();
060    public int currentPipelineIndex = 0;
061
062    public int streamIndex = 0; // 0 index means ports [1181, 1182], 1 means [1183, 1184], etc...
063
064    // Ignore the pipes, as we serialize them to their own column to hack around
065    // polymorphic lists
066    @JsonIgnore public List<CVPipelineSettings> pipelineSettings = new ArrayList<>();
067
068    @JsonIgnore
069    public DriverModePipelineSettings driveModeSettings = new DriverModePipelineSettings();
070
071    public CameraConfiguration(PVCameraInfo cameraInfo, String uniqueName, String nickname) {
072        this.matchedCameraInfo = cameraInfo;
073        this.uniqueName = uniqueName;
074        this.nickname = nickname;
075        this.calibrations = new ArrayList<>();
076
077        logger.debug("Creating USB camera configuration for " + this.toShortString());
078    }
079
080    // Shiny new constructor
081    @JsonCreator
082    public CameraConfiguration(
083            @JsonProperty("uniqueName") String uniqueName,
084            @JsonProperty("matchedCameraInfo") PVCameraInfo matchedCameraInfo,
085            @JsonProperty("nickname") String nickname,
086            @JsonProperty("deactivated") boolean deactivated,
087            @JsonProperty("cameraQuirks") QuirkyCamera cameraQuirks,
088            @JsonProperty("FOV") double FOV,
089            @JsonProperty("calibrations") List<CameraCalibrationCoefficients> calibrations,
090            @JsonProperty("currentPipelineIndex") int currentPipelineIndex) {
091        this.uniqueName = uniqueName;
092        this.matchedCameraInfo = matchedCameraInfo;
093        this.nickname = nickname;
094        this.deactivated = deactivated;
095        this.cameraQuirks = cameraQuirks;
096        this.FOV = FOV;
097        this.calibrations = calibrations != null ? calibrations : new ArrayList<>();
098        this.currentPipelineIndex = currentPipelineIndex;
099    }
100
101    // Special case constructor for use with File sources
102    public CameraConfiguration(String uniqueName, PVCameraInfo camInfo) {
103        this.uniqueName = uniqueName;
104        this.matchedCameraInfo = camInfo;
105        this.nickname = camInfo.humanReadableName();
106        this.calibrations = new ArrayList<>();
107        this.cameraQuirks = null; // we'll deal with this later. TODO: should we not just do it now?
108    }
109
110    /**
111     * Constructor for when we don't know anything about the camera yet. Generates a UUID for the
112     * unique name
113     */
114    public CameraConfiguration(PVCameraInfo camInfo) {
115        this(UUID.randomUUID().toString(), camInfo);
116    }
117
118    public static class LegacyCameraConfigStruct {
119        PVCameraInfo matchedCameraInfo;
120
121        /** Legacy constructor for compat with 2024.3.1 */
122        @JsonCreator
123        public LegacyCameraConfigStruct(
124                @JsonProperty("baseName") String baseName,
125                @JsonProperty("path") String path,
126                @JsonProperty("otherPaths") String[] otherPaths,
127                @JsonProperty("cameraType") CameraType cameraType,
128                @JsonProperty("usbVID") int usbVID,
129                @JsonProperty("usbPID") int usbPID) {
130            if (cameraType == CameraType.UsbCamera) {
131                this.matchedCameraInfo =
132                        PVCameraInfo.fromUsbCameraInfo(
133                                new UsbCameraInfo(-1, path, baseName, otherPaths, usbVID, usbPID));
134            } else if (cameraType == CameraType.ZeroCopyPicam) {
135                this.matchedCameraInfo = PVCameraInfo.fromCSICameraInfo(path, baseName);
136            } else {
137                // wtf
138                logger.error("Camera type is invalid");
139                this.matchedCameraInfo = null;
140                return;
141            }
142        }
143    }
144
145    public void addPipelineSettings(List<CVPipelineSettings> settings) {
146        for (var setting : settings) {
147            addPipelineSetting(setting);
148        }
149    }
150
151    public void addPipelineSetting(CVPipelineSettings setting) {
152        if (pipelineSettings.stream()
153                .anyMatch(s -> s.pipelineNickname.equalsIgnoreCase(setting.pipelineNickname))) {
154            logger.error("Could not name two pipelines the same thing! Renaming");
155            setting.pipelineNickname += "_1"; // TODO verify this logic
156        }
157
158        if (pipelineSettings.stream().anyMatch(s -> s.pipelineIndex == setting.pipelineIndex)) {
159            var newIndex = pipelineSettings.size();
160            logger.error("Could not insert two pipelines at same index! Changing to " + newIndex);
161            setting.pipelineIndex = newIndex; // TODO verify this logic
162        }
163
164        pipelineSettings.add(setting);
165        pipelineSettings.sort(PipelineManager.PipelineSettingsIndexComparator);
166    }
167
168    public void setPipelineSettings(List<CVPipelineSettings> settings) {
169        pipelineSettings = settings;
170    }
171
172    /**
173     * Replace a calibration in our list with the same unrotatedImageSize with a new one, or add it if
174     * none exists yet. If we are replacing an existing calibration, the old one will be "released"
175     * and the underlying data matrices will become invalid.
176     *
177     * @param calibration The calibration to add.
178     */
179    public void addCalibration(CameraCalibrationCoefficients calibration) {
180        logger.info("adding calibration " + calibration.unrotatedImageSize);
181        calibrations.stream()
182                .filter(it -> it.unrotatedImageSize.equals(calibration.unrotatedImageSize))
183                .findAny()
184                .ifPresent(
185                        (it) -> {
186                            it.release();
187                            calibrations.remove(it);
188                        });
189        calibrations.add(calibration);
190    }
191
192    /**
193     * cscore will auto-reconnect to the camera path we give it. v4l does not guarantee that if i swap
194     * cameras around, the same /dev/videoN ID will be assigned to that camera. So instead default to
195     * pinning to a particular USB port, or by "path" (appears to be a global identifier on Windows).
196     *
197     * <p>This represents our best guess at an immutable path to detect a camera at.
198     */
199    @JsonIgnore
200    public String getDevicePath() {
201        return matchedCameraInfo.uniquePath();
202    }
203
204    public String toShortString() {
205        return "CameraConfiguration [uniqueName="
206                + uniqueName
207                + ", matchedCameraInfo="
208                + matchedCameraInfo
209                + ", nickname="
210                + nickname
211                + ", deactivated="
212                + deactivated
213                + ", cameraQuirks="
214                + cameraQuirks
215                + ", FOV="
216                + FOV
217                + "]";
218    }
219
220    @Override
221    public String toString() {
222        return "CameraConfiguration [uniqueName="
223                + uniqueName
224                + ", matchedCameraInfo="
225                + matchedCameraInfo
226                + ", nickname="
227                + nickname
228                + ", deactivated="
229                + deactivated
230                + ", cameraQuirks="
231                + cameraQuirks
232                + ", FOV="
233                + FOV
234                + ", calibrations="
235                + calibrations
236                + ", currentPipelineIndex="
237                + currentPipelineIndex
238                + ", streamIndex="
239                + streamIndex
240                + ", pipelineSettings="
241                + pipelineSettings
242                + ", driveModeSettings="
243                + driveModeSettings
244                + "]";
245    }
246
247    /**
248     * UICameraConfiguration has some stuff particular to VisionModule, but enough of it's common to
249     * warrant this helper
250     */
251    public UICameraConfiguration toUiConfig() {
252        var ret = new UICameraConfiguration();
253
254        ret.matchedCameraInfo = matchedCameraInfo;
255        ret.cameraPath = getDevicePath();
256        ret.nickname = nickname;
257        ret.uniqueName = uniqueName;
258        ret.deactivated = deactivated;
259        ret.isCSICamera = matchedCameraInfo.type() == CameraType.ZeroCopyPicam;
260        ret.pipelineNicknames = pipelineSettings.stream().map(it -> it.pipelineNickname).toList();
261        ret.cameraQuirks = cameraQuirks;
262        ret.calibrations =
263                calibrations.stream().map(CameraCalibrationCoefficients::cloneWithoutObservations).toList();
264
265        return ret;
266    }
267}