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.util;
019
020import com.fasterxml.jackson.databind.ObjectMapper;
021import edu.wpi.first.math.geometry.Translation2d;
022import edu.wpi.first.math.util.Units;
023import java.awt.HeadlessException;
024import java.io.File;
025import java.io.IOException;
026import java.nio.file.Path;
027import org.opencv.core.Mat;
028import org.opencv.highgui.HighGui;
029import org.photonvision.jni.WpilibLoader;
030import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
031
032public class TestUtils {
033    public static boolean loadLibraries() {
034        return WpilibLoader.loadLibraries();
035    }
036
037    @SuppressWarnings("unused")
038    public enum WPI2019Image {
039        kCargoAngledDark48in(1.2192),
040        kCargoSideStraightDark36in(0.9144),
041        kCargoSideStraightDark60in(1.524),
042        kCargoSideStraightDark72in(1.8288),
043        kCargoSideStraightPanelDark36in(0.9144),
044        kCargoStraightDark19in(0.4826),
045        kCargoStraightDark24in(0.6096),
046        kCargoStraightDark48in(1.2192),
047        kCargoStraightDark72in(1.8288),
048        kCargoStraightDark72in_HighRes(1.8288),
049        kCargoStraightDark90in(2.286),
050        kRocketPanelAngleDark48in(1.2192),
051        kRocketPanelAngleDark60in(1.524);
052
053        public static double FOV = 68.5;
054
055        public final double distanceMeters;
056        public final Path path;
057
058        Path getPath() {
059            var filename = this.toString().substring(1);
060            return Path.of("2019", "WPI", filename + ".jpg");
061        }
062
063        WPI2019Image(double distanceMeters) {
064            this.distanceMeters = distanceMeters;
065            this.path = getPath();
066        }
067    }
068
069    @SuppressWarnings("unused")
070    public enum WPI2020Image {
071        kBlueGoal_060in_Center(1.524),
072        kBlueGoal_084in_Center(2.1336),
073        kBlueGoal_084in_Center_720p(2.1336),
074        kBlueGoal_108in_Center(2.7432),
075        kBlueGoal_132in_Center(3.3528),
076        kBlueGoal_156in_Center(3.9624),
077        kBlueGoal_180in_Center(4.572),
078        kBlueGoal_156in_Left(3.9624),
079        kBlueGoal_224in_Left(5.6896),
080        kBlueGoal_228in_ProtectedZone(5.7912),
081        kBlueGoal_330in_ProtectedZone(8.382),
082        kBlueGoal_Far_ProtectedZone(10.668), // TODO: find a more accurate distance
083        kRedLoading_016in_Down(0.4064),
084        kRedLoading_030in_Down(0.762),
085        kRedLoading_048in_Down(1.2192),
086        kRedLoading_048in(1.2192),
087        kRedLoading_060in(1.524),
088        kRedLoading_084in(2.1336),
089        kRedLoading_108in(2.7432);
090
091        public static double FOV = 68.5;
092
093        public final double distanceMeters;
094        public final Path path;
095
096        Path getPath() {
097            var filename = this.toString().substring(1).replace('_', '-');
098            return Path.of("2020", "WPI", filename + ".jpg");
099        }
100
101        WPI2020Image(double distanceMeters) {
102            this.distanceMeters = distanceMeters;
103            this.path = getPath();
104        }
105    }
106
107    public enum WPI2024Images {
108        kBackAmpZone_117in,
109        kSpeakerCenter_143in;
110
111        public static double FOV = 68.5;
112
113        public final Path path;
114
115        Path getPath() {
116            var filename = this.toString().substring(1);
117            return Path.of("2024", filename + ".jpg");
118        }
119
120        WPI2024Images() {
121            this.path = getPath();
122        }
123    }
124
125    public enum WPI2023Apriltags {
126        k162_36_Angle,
127        k162_36_Straight,
128        k383_60_Angle2;
129
130        public static double FOV = 68.5;
131
132        public final Translation2d approxPose;
133        public final Path path;
134
135        Path getPath() {
136            var filename = this.toString().substring(1);
137            return Path.of("2023", "AprilTags", filename + ".png");
138        }
139
140        Translation2d getPose() {
141            var names = this.toString().substring(1).split("_");
142            var x = Units.inchesToMeters(Integer.parseInt(names[0]));
143            var y = Units.inchesToMeters(Integer.parseInt(names[1]));
144            return new Translation2d(x, y);
145        }
146
147        WPI2023Apriltags() {
148            this.approxPose = getPose();
149            this.path = getPath();
150        }
151    }
152
153    public enum WPI2022Image {
154        kTerminal12ft6in(Units.feetToMeters(12.5)),
155        kTerminal22ft6in(Units.feetToMeters(22.5));
156
157        public static double FOV = 68.5;
158
159        public final double distanceMeters;
160        public final Path path;
161
162        Path getPath() {
163            var filename = this.toString().substring(1).replace('_', '-');
164            return Path.of("2022", "WPI", filename + ".png");
165        }
166
167        WPI2022Image(double distanceMeters) {
168            this.distanceMeters = distanceMeters;
169            this.path = getPath();
170        }
171    }
172
173    public enum PolygonTestImages {
174        kPolygons;
175
176        public final Path path;
177
178        Path getPath() {
179            var filename = this.toString().substring(1).toLowerCase();
180            return Path.of("polygons", filename + ".png");
181        }
182
183        PolygonTestImages() {
184            this.path = getPath();
185        }
186    }
187
188    public enum PowercellTestImages {
189        kPowercell_test_1,
190        kPowercell_test_2,
191        kPowercell_test_3,
192        kPowercell_test_4,
193        kPowercell_test_5,
194        kPowercell_test_6;
195
196        public final Path path;
197
198        Path getPath() {
199            var filename = this.toString().substring(1).toLowerCase();
200            return Path.of(filename + ".png");
201        }
202
203        PowercellTestImages() {
204            this.path = getPath();
205        }
206    }
207
208    public enum ApriltagTestImages {
209        kRobots,
210        kTag1_640_480,
211        kTag1_16h5_1280,
212        kTag_corner_1280;
213
214        public final Path path;
215
216        Path getPath() {
217            // Strip leading k
218            var filename = this.toString().substring(1).toLowerCase();
219            var extension = ".jpg";
220            if (filename.equals("tag1_16h5_1280")) extension = ".png";
221            return Path.of("apriltag", filename + extension);
222        }
223
224        ApriltagTestImages() {
225            this.path = getPath();
226        }
227    }
228
229    public static Path getResourcesFolderPath(boolean testMode) {
230        System.out.println("CWD: " + Path.of("").toAbsolutePath());
231
232        // VSCode likes to make this path relative to the wrong root directory, so a fun hack to tell
233        // if it's wrong
234        Path ret = Path.of("test-resources").toAbsolutePath();
235        if (Path.of("test-resources")
236                .toAbsolutePath()
237                .toString()
238                .replace("/", "")
239                .replace("\\", "")
240                .toLowerCase()
241                .matches(".*photon-[a-z]*test-resources")) {
242            ret = Path.of("../test-resources").toAbsolutePath();
243        }
244        return ret;
245    }
246
247    public static Path getTestMode2019ImagePath() {
248        return getResourcesFolderPath(true)
249                .resolve("testimages")
250                .resolve(WPI2019Image.kRocketPanelAngleDark60in.path);
251    }
252
253    public static Path getTestMode2020ImagePath() {
254        return getResourcesFolderPath(true)
255                .resolve("testimages")
256                .resolve(WPI2020Image.kBlueGoal_156in_Left.path);
257    }
258
259    public static Path getTestMode2022ImagePath() {
260        return getResourcesFolderPath(true)
261                .resolve("testimages")
262                .resolve(WPI2022Image.kTerminal22ft6in.path);
263    }
264
265    public static Path getTestModeApriltagPath() {
266        return getResourcesFolderPath(true)
267                .resolve("testimages")
268                .resolve(ApriltagTestImages.kRobots.path);
269    }
270
271    public static Path getTestImagesPath(boolean testMode) {
272        return getResourcesFolderPath(testMode).resolve("testimages");
273    }
274
275    public static Path getCalibrationPath(boolean testMode) {
276        return getResourcesFolderPath(testMode).resolve("calibration");
277    }
278
279    public static Path getPowercellPath(boolean testMode) {
280        return getTestImagesPath(testMode).resolve("polygons").resolve("powercells");
281    }
282
283    public static Path getWPIImagePath(WPI2020Image image, boolean testMode) {
284        return getTestImagesPath(testMode).resolve(image.path);
285    }
286
287    public static Path getWPIImagePath(WPI2019Image image, boolean testMode) {
288        return getTestImagesPath(testMode).resolve(image.path);
289    }
290
291    public static Path getPolygonImagePath(PolygonTestImages image, boolean testMode) {
292        return getTestImagesPath(testMode).resolve(image.path);
293    }
294
295    public static Path getApriltagImagePath(ApriltagTestImages image, boolean testMode) {
296        return getTestImagesPath(testMode).resolve(image.path);
297    }
298
299    public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) {
300        return getPowercellPath(testMode).resolve(image.path);
301    }
302
303    public static Path getSquaresBoardImagesPath() {
304        return getResourcesFolderPath(false).resolve("calibrationSquaresImg");
305    }
306
307    public static Path getCharucoBoardImagesPath() {
308        return getResourcesFolderPath(false).resolve("calibrationCharucoImg");
309    }
310
311    public static File getHardwareConfigJson() {
312        return getResourcesFolderPath(false)
313                .resolve("hardware")
314                .resolve("HardwareConfig.json")
315                .toFile();
316    }
317
318    private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
319    private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
320    public static final String LIFECAM_1280P_CAL_FILE = "lifecam_1280.json";
321    public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json";
322
323    public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
324        try {
325            return new ObjectMapper()
326                    .readValue(
327                            (Path.of(getCalibrationPath(testMode).toString(), filename).toFile()),
328                            CameraCalibrationCoefficients.class);
329        } catch (IOException e) {
330            e.printStackTrace();
331            return null;
332        }
333    }
334
335    public static CameraCalibrationCoefficients get2019LifeCamCoeffs(boolean testMode) {
336        return getCoeffs(LIFECAM_240P_CAL_FILE, testMode);
337    }
338
339    public static CameraCalibrationCoefficients get2020LifeCamCoeffs(boolean testMode) {
340        return getCoeffs(LIFECAM_480P_CAL_FILE, testMode);
341    }
342
343    public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
344        return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
345    }
346
347    public static CameraCalibrationCoefficients getLaptop() {
348        return getCoeffs("laptop.json", true);
349    }
350
351    private static final int DefaultTimeoutMillis = 5000;
352
353    public static void showImage(Mat frame, String title, int timeoutMs) {
354        if (frame.empty()) return;
355        try {
356            HighGui.imshow(title, frame);
357            HighGui.waitKey(timeoutMs);
358            HighGui.destroyAllWindows();
359        } catch (HeadlessException ignored) {
360        }
361    }
362
363    public static void showImage(Mat frame, int timeoutMs) {
364        showImage(frame, "", timeoutMs);
365    }
366
367    public static void showImage(Mat frame, String title) {
368        showImage(frame, title, DefaultTimeoutMillis);
369    }
370
371    public static void showImage(Mat frame) {
372        showImage(frame, DefaultTimeoutMillis);
373    }
374
375    public static Path getTestMode2023ImagePath() {
376        return getResourcesFolderPath(true)
377                .resolve("testimages")
378                .resolve(WPI2022Image.kTerminal22ft6in.path);
379    }
380
381    public static Path getConfigDirectoriesPath(boolean testMode) {
382        return getResourcesFolderPath(testMode).resolve("old_configs");
383    }
384}