From 9b183ebb85be398f2e21474e3d18c0640027ea0f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 21:24:49 -0500 Subject: [PATCH 01/20] Add option to view streams fullscreen (#1006) closes #1005 --- .../components/app/photon-camera-stream.vue | 32 +++++++++++++------ .../src/components/cameras/CamerasView.vue | 14 ++++++-- 2 files changed, 35 insertions(+), 11 deletions(-) diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 13f3e75776..5485655ff5 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -8,7 +8,7 @@ import PvIcon from "@/components/common/pv-icon.vue"; const props = defineProps<{ streamType: "Raw" | "Processed"; - id?: string; + id: string; }>(); const streamSrc = computed(() => { @@ -25,8 +25,6 @@ const streamDesc = computed(() => `${props.streamType} Stream View`); const streamStyle = computed(() => { if (useStateStore().colorPickingMode) { return { width: "100%", cursor: "crosshair" }; - } else if (streamSrc.value !== loadingImage) { - return { width: "100%", cursor: "pointer" }; } return { width: "100%" }; @@ -40,11 +38,6 @@ const overlayStyle = computed(() => { } }); -const handleStreamClick = () => { - if (!useStateStore().colorPickingMode && streamSrc.value !== loadingImage) { - window.open(streamSrc.value); - } -}; const handleCaptureClick = () => { if (props.streamType === "Raw") { useCameraSettingsStore().saveInputSnapshot(); @@ -52,6 +45,14 @@ const handleCaptureClick = () => { useCameraSettingsStore().saveOutputSnapshot(); } }; +const handlePopoutClick = () => { + window.open(streamSrc.value); +}; +const handleFullscreenRequest = () => { + const stream = document.getElementById(props.id); + if (!stream) return; + stream.requestFullscreen(); +}; @@ -81,6 +94,7 @@ const handleCaptureClick = () => { } .stream-overlay { + display: flex; opacity: 0; transition: 0.1s ease; position: absolute; diff --git a/photon-client/src/components/cameras/CamerasView.vue b/photon-client/src/components/cameras/CamerasView.vue index f7c110cf62..1d52bfcf05 100644 --- a/photon-client/src/components/cameras/CamerasView.vue +++ b/photon-client/src/components/cameras/CamerasView.vue @@ -78,10 +78,20 @@ const fpsTooLow = computed(() => {
- +
- +
From 5a9cf418d460fc41cbe72845103df5802b20f47f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 22:15:08 -0500 Subject: [PATCH 02/20] Fix bug with always opening stream fullscreen onclick (#1018) * Update photon-camera-stream.vue * formatting fixes --- .../src/components/app/photon-camera-stream.vue | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 5485655ff5..a836e4b362 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -57,14 +57,7 @@ const handleFullscreenRequest = () => { @@ -73,7 +73,7 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) + "%" : "(In Multi-Target)" }} + {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) : "(In Multi-Target)" }} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index dc847ec5fc..392a429006 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -97,8 +97,9 @@ public int getFiducialId() { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is betweeen 0 + * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers + * above 0.2 are likely to be ambiguous. -1 if invalid. */ public double getPoseAmbiguity() { return poseAmbiguity; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 7eb18d125f..feba565910 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -112,8 +112,10 @@ class PhotonTrackedTarget { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above - * 0.2 are likely to be ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. + * This is betweeen 0 and 1 (0 being no ambiguity, and 1 meaning both have the + * same reprojection error). Numbers above 0.2 are likely to be ambiguous. -1 + * if invalid. */ double GetPoseAmbiguity() const { return poseAmbiguity; } From 7b49570e9d6caa64a5cf3ad33fd6ef587446acf8 Mon Sep 17 00:00:00 2001 From: Joseph Farkas <16584585+MrRedness@users.noreply.github.com> Date: Mon, 27 Nov 2023 23:43:57 -0500 Subject: [PATCH 07/20] Radians to Degrees for multi-tag pose (#1027) * Radians to Degrees for multi-tag pose * fix optional check --------- Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> --- .../src/components/dashboard/tabs/TargetsTab.vue | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue index 45975da6d6..ac6aa632c4 100644 --- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue +++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue @@ -73,7 +73,7 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) : "(In Multi-Target)" }} + {{ target.ambiguity >= 0 ? target.ambiguity.toFixed(2) : "(In Multi-Target)" }} @@ -102,7 +102,14 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }} m {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }} m - {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z.toFixed(2) }}° + + {{ + ( + useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z * + (180.0 / Math.PI) + ).toFixed(2) + }}° + {{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }} From f597d111b320958a7b44d8eedd7fef61b5d35a7b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 29 Nov 2023 18:32:04 -0500 Subject: [PATCH 08/20] Update bug issue template with UI specific comment (#1033) * Update bug_report.md * Update bug_report.md --- .github/ISSUE_TEMPLATE/bug_report.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index d5d59cc6f1..5cdc15f846 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -18,7 +18,7 @@ Steps to reproduce the behavior: 4. See error **Screenshots / Videos** -If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. +If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. If your issue is regarding the web dashboard, please provide screenshots and the output of the browser console. **Platform:** - Hardware Platform (ex. Raspberry Pi 4, Windows x64): From 469bc0eeaeee19433c70782b84e452d1fce34d21 Mon Sep 17 00:00:00 2001 From: Programmers3539 Date: Fri, 1 Dec 2023 07:58:38 -0500 Subject: [PATCH 09/20] Fix Camera Index Assignment. (#1031) Fixes a bug where multiple cameras would receive identical stream indexes and would cause at least one camera to not function at all and the other to not display a stream. One of the reasons the that usbcamerasource equals method used to incorrectly determine equality was because the quirkycamera object didn't have a basename. When a camera that had a quirk was found it would set equal to a predefined quirkycamera without changing the name first. Add unit test that will better test the equality of two usbcamerasources. This required a few changes to allow creating a fake usbcamerasource. --- .../vision/camera/QuirkyCamera.java | 16 +++++- .../vision/camera/USBCameraSource.java | 54 ++++++++++++++++--- .../vision/processes/VisionModule.java | 2 +- .../vision/processes/VisionSourceManager.java | 2 +- .../processes/VisionSourceSettables.java | 2 +- .../processes/VisionModuleManagerTest.java | 18 ++++++- 6 files changed, 81 insertions(+), 13 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 53d89167ec..05737ed293 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -17,6 +17,8 @@ package org.photonvision.vision.camera; +import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Objects; @@ -108,8 +110,20 @@ public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseNa for (var qc : quirkyCameras) { boolean hasBaseName = !qc.baseName.isEmpty(); boolean matchesBaseName = qc.baseName.equals(baseName) || !hasBaseName; + // If we have a quirkycamera we need to copy the quirks from our predefined object and create + // a quirkycamera object with the baseName. if (qc.usbVid == usbVid && qc.usbPid == usbPid && matchesBaseName) { - return qc; + List quirks = new ArrayList(); + for (var q : CameraQuirk.values()) { + if (qc.hasQuirk(q)) quirks.add(q); + } + QuirkyCamera c = + new QuirkyCamera( + usbVid, + usbPid, + baseName, + Arrays.copyOf(quirks.toArray(), quirks.size(), CameraQuirk[].class)); + return c; } } return new QuirkyCamera(usbVid, usbPid, baseName); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index 6717f4edd8..a2f2911431 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -28,7 +28,9 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.FrameProvider; +import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; @@ -37,10 +39,10 @@ public class USBCameraSource extends VisionSource { private final Logger logger; private final UsbCamera camera; private final USBCameraSettables usbCameraSettables; - private final USBFrameProvider usbFrameProvider; + private FrameProvider usbFrameProvider; private final CvSink cvSink; - public final QuirkyCamera cameraQuirks; + private QuirkyCamera cameraQuirks; public USBCameraSource(CameraConfiguration config) { super(config); @@ -77,6 +79,22 @@ public USBCameraSource(CameraConfiguration config) { } } + /** + * Mostly just used for unit tests to better simulate a usb camera without a camera being present. + */ + public USBCameraSource(CameraConfiguration config, int pid, int vid, boolean unitTest) { + this(config); + + cameraQuirks = QuirkyCamera.getQuirkyCamera(pid, vid, config.baseName); + + if (unitTest) + usbFrameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath( + TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + } + void disableAutoFocus() { if (cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) { try { @@ -88,6 +106,10 @@ void disableAutoFocus() { } } + public QuirkyCamera getCameraQuirks() { + return this.cameraQuirks; + } + @Override public FrameProvider getFrameProvider() { return usbFrameProvider; @@ -103,7 +125,7 @@ protected USBCameraSettables(CameraConfiguration configuration) { super(configuration); getAllVideoModes(); if (!cameraQuirks.hasQuirk(CameraQuirk.StickyFPS)) - setVideoMode(videoModes.get(0)); // fixes double FPS set + if (!videoModes.isEmpty()) setVideoMode(videoModes.get(0)); // fixes double FPS set } public void setAutoExposure(boolean cameraAutoExposure) { @@ -343,11 +365,27 @@ public boolean isVendorCamera() { } @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - USBCameraSource that = (USBCameraSource) o; - return cameraQuirks.equals(that.cameraQuirks); + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + USBCameraSource other = (USBCameraSource) obj; + if (camera == null) { + if (other.camera != null) return false; + } else if (!camera.equals(other.camera)) return false; + if (usbCameraSettables == null) { + if (other.usbCameraSettables != null) return false; + } else if (!usbCameraSettables.equals(other.usbCameraSettables)) return false; + if (usbFrameProvider == null) { + if (other.usbFrameProvider != null) return false; + } else if (!usbFrameProvider.equals(other.usbFrameProvider)) return false; + if (cvSink == null) { + if (other.cvSink != null) return false; + } else if (!cvSink.equals(other.cvSink)) return false; + if (cameraQuirks == null) { + if (other.cameraQuirks != null) return false; + } else if (!cameraQuirks.equals(other.cameraQuirks)) return false; + return true; } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 976053396b..483f6a7c0c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -95,7 +95,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, // Find quirks for the current camera if (visionSource instanceof USBCameraSource) { - cameraQuirks = ((USBCameraSource) visionSource).cameraQuirks; + cameraQuirks = ((USBCameraSource) visionSource).getCameraQuirks(); } else if (visionSource instanceof LibcameraGpuSource) { cameraQuirks = QuirkyCamera.ZeroCopyPiCamera; } else { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 4b31ba0691..c16e91e025 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -342,7 +342,7 @@ private static List loadVisionSourcesFromCamConfigs( cameraSources.add(piCamSrc); } else { var newCam = new USBCameraSource(configuration); - if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken) + if (!newCam.getCameraQuirks().hasQuirk(CameraQuirk.CompletelyBroken) && !newCam.getSettables().videoModes.isEmpty()) { cameraSources.add(newCam); } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index cda7d08e6f..f2057a18db 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -59,7 +59,7 @@ public void setBlueGain(int blue) {} public abstract VideoMode getCurrentVideoMode(); public void setVideoModeInternal(int index) { - setVideoMode(getAllVideoModes().get(index)); + if (!getAllVideoModes().isEmpty()) setVideoMode(getAllVideoModes().get(index)); } public void setVideoMode(VideoMode mode) { diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index e0a8376159..9b4ca528b6 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -31,6 +31,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.util.TestUtils; +import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.frame.provider.FileFrameProvider; @@ -165,7 +166,16 @@ public void testMultipleStreamIndex() { TestUtils.WPI2019Image.FOV); var testSource3 = new TestSource(ffp3, conf3); - var modules = vmm.addSources(List.of(testSource, testSource2, testSource3)); + // Arducam OV9281 UC844 raspberry pi test. + var conf4 = new CameraConfiguration("Left", "dev/video1"); + USBCameraSource usbSimulation = new USBCameraSource(conf4, 0x6366, 0x0c45, true); + + var conf5 = new CameraConfiguration("Right", "dev/video2"); + USBCameraSource usbSimulation2 = new USBCameraSource(conf5, 0x6366, 0x0c45, true); + + var modules = + vmm.addSources( + List.of(testSource, testSource2, testSource3, usbSimulation, usbSimulation2)); System.out.println( Arrays.toString( @@ -176,9 +186,15 @@ public void testMultipleStreamIndex() { modules.stream() .map(it -> it.visionSource.getCameraConfiguration().streamIndex) .collect(Collectors.toList()); + + assertTrue(usbSimulation.equals(usbSimulation)); + assertTrue(!usbSimulation.equals(usbSimulation2)); + assertTrue(idxs.contains(0)); assertTrue(idxs.contains(1)); assertTrue(idxs.contains(2)); + assertTrue(idxs.contains(3)); + assertTrue(idxs.contains(4)); } private static void printTestResults(CVPipelineResult pipelineResult) { From 6db5bc5e0c439a2e794fb1b36e0840fc67642fc6 Mon Sep 17 00:00:00 2001 From: Joseph Farkas <16584585+MrRedness@users.noreply.github.com> Date: Mon, 4 Dec 2023 16:55:16 +0000 Subject: [PATCH 10/20] Matching cameras by path ID (#1015) Allows multiple cameras of the same model to be used while ensuring they stay tied to the physical camera and not the port. Matching occurs when first connecting cameras so swapping the ports while PV is running will swap the virtual cameras until a restart. Currently only tested on Linux. --- .../configuration/SqlConfigProvider.java | 15 +- .../vision/processes/VisionSourceManager.java | 152 ++++++++++++++---- .../processes/VisionSourceManagerTest.java | 81 +++++++++- 3 files changed, 207 insertions(+), 41 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 8a0c755ad8..3f22e69522 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -51,6 +51,7 @@ static class TableKeys { static final String CAM_UNIQUE_NAME = "unique_name"; static final String CONFIG_JSON = "config_json"; static final String DRIVERMODE_JSON = "drivermode_json"; + static final String OTHERPATHS_JSON = "otherpaths_json"; static final String PIPELINE_JSONS = "pipeline_jsons"; static final String NETWORK_CONFIG = "networkConfig"; @@ -147,6 +148,7 @@ private void initDatabase() { + " unique_name TINYTEXT PRIMARY KEY,\n" + " config_json text NOT NULL,\n" + " drivermode_json text NOT NULL,\n" + + " otherpaths_json text NOT NULL,\n" + " pipeline_jsons mediumtext NOT NULL\n" + ");"; createCameraTableStatement.execute(sql); @@ -295,8 +297,8 @@ private void saveCameras(Connection conn) { try { // Replace this camera's row with the new settings var sqlString = - "REPLACE INTO cameras (unique_name, config_json, drivermode_json, pipeline_jsons) VALUES " - + "(?,?,?,?);"; + "REPLACE INTO cameras (unique_name, config_json, drivermode_json, otherpaths_json, pipeline_jsons) VALUES " + + "(?,?,?,?,?);"; for (var c : config.getCameraConfigurations().entrySet()) { PreparedStatement statement = conn.prepareStatement(sqlString); @@ -305,6 +307,7 @@ private void saveCameras(Connection conn) { statement.setString(1, c.getKey()); statement.setString(2, JacksonUtils.serializeToString(config)); statement.setString(3, JacksonUtils.serializeToString(config.driveModeSettings)); + statement.setString(4, JacksonUtils.serializeToString(config.otherPaths)); // Serializing a list of abstract classes sucks. Instead, make it into an array // of strings, which we can later unpack back into individual settings @@ -321,7 +324,7 @@ private void saveCameras(Connection conn) { }) .filter(Objects::nonNull) .collect(Collectors.toList()); - statement.setString(4, JacksonUtils.serializeToString(settings)); + statement.setString(5, JacksonUtils.serializeToString(settings)); statement.executeUpdate(); } @@ -455,10 +458,11 @@ private HashMap loadCameraConfigs(Connection conn) query = conn.prepareStatement( String.format( - "SELECT %s, %s, %s, %s FROM cameras", + "SELECT %s, %s, %s, %s, %s FROM cameras", TableKeys.CAM_UNIQUE_NAME, TableKeys.CONFIG_JSON, TableKeys.DRIVERMODE_JSON, + TableKeys.OTHERPATHS_JSON, TableKeys.PIPELINE_JSONS)); var result = query.executeQuery(); @@ -474,6 +478,8 @@ private HashMap loadCameraConfigs(Connection conn) var driverMode = JacksonUtils.deserialize( result.getString(TableKeys.DRIVERMODE_JSON), DriverModePipelineSettings.class); + var otherPaths = + JacksonUtils.deserialize(result.getString(TableKeys.OTHERPATHS_JSON), String[].class); List pipelineSettings = JacksonUtils.deserialize( result.getString(TableKeys.PIPELINE_JSONS), dummyList.getClass()); @@ -487,6 +493,7 @@ private HashMap loadCameraConfigs(Connection conn) config.pipelineSettings = loadedSettings; config.driveModeSettings = driverMode; + config.otherPaths = otherPaths; loadedConfigurations.put(uniqueName, config); } } catch (SQLException | IOException e) { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index c16e91e025..2e6ada0aa5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -196,52 +196,112 @@ protected List tryMatchUSBCamImpl(boolean createSources) { * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. * @return the matched configurations. */ - private List matchUSBCameras( + protected List matchUSBCameras( List detectedCamInfos, List loadedUsbCamConfigs) { var detectedCameraList = new ArrayList<>(detectedCamInfos); ArrayList cameraConfigurations = new ArrayList<>(); - // loop over all the configs loaded from disk + List unmatchedAfterByID = new ArrayList<>(loadedUsbCamConfigs); + + // loop over all the configs loaded from disk, attempting to match each camera + // to a config by path-by-id on linux for (CameraConfiguration config : loadedUsbCamConfigs) { UsbCameraInfo cameraInfo; - // attempt matching by path and basename - logger.debug( - "Trying to find a match for loaded camera " - + config.baseName - + " with path " - + config.path); - cameraInfo = - detectedCameraList.stream() - .filter( - usbCameraInfo -> - usbCameraInfo.path.equals(config.path) - && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) - .findFirst() - .orElse(null); - - // if path based fails, attempt basename only match - if (cameraInfo == null) { - logger.debug("Failed to match by path and name, falling back to name-only match"); + if (config.otherPaths.length == 0) { + logger.debug("No valid path-by-id found for config with name " + config.baseName); + } else { + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path-by-id " + + config.otherPaths[0]); + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + usbCameraInfo.otherPaths.length != 0 + && usbCameraInfo.otherPaths[0].equals(config.otherPaths[0]) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByID.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } + } + + if (!unmatchedAfterByID.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by path-by-id failed, falling back to path-only matching"); + + List unmatchedAfterByPath = new ArrayList<>(loadedUsbCamConfigs); + + // now attempt to match the cameras and configs remaining by normal path + for (CameraConfiguration config : unmatchedAfterByID) { + UsbCameraInfo cameraInfo; + + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path " + + config.path); cameraInfo = detectedCameraList.stream() .filter( usbCameraInfo -> - cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + usbCameraInfo.path.equals(config.path) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) .findFirst() .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByPath.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } } - // If we actually matched a camera to a config, remove that camera from the list and add it to - // the output - if (cameraInfo != null) { - logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); - detectedCameraList.remove(cameraInfo); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + if (!unmatchedAfterByPath.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by ID and path failed, falling back to name-only matching"); + + // if both path and ID based matching fails, attempt basename only match + for (CameraConfiguration config : unmatchedAfterByPath) { + UsbCameraInfo cameraInfo; + + logger.debug("Trying to find a match for loaded camera with name " + config.baseName); + + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } } } - // If we have any unmatched cameras left, create a new CameraConfiguration for them here. + // If we have any unmatched cameras left, create a new CameraConfiguration for + // them here. logger.debug( "After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched."); for (UsbCameraInfo info : detectedCameraList) { @@ -250,7 +310,7 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) String uniqueName = baseNameToUniqueName(baseName); int suffix = 0; - while (containsName(cameraConfigurations, uniqueName)) { + while (containsName(cameraConfigurations, uniqueName) || containsName(uniqueName)) { suffix++; uniqueName = String.format("%s (%d)", uniqueName, suffix); } @@ -283,6 +343,27 @@ private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, UsbCame cfg.path = info.path; } + if (cfg.otherPaths.length != info.otherPaths.length) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + } else { + for (int i = 0; i < info.otherPaths.length; i++) { + if (!cfg.otherPaths[i].equals(info.otherPaths[i])) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + break; + } + } + } + return cfg; } @@ -309,7 +390,7 @@ private List filterAllowedDevices(List allDevices) private boolean usbCamEquals(UsbCameraInfo a, UsbCameraInfo b) { return a.path.equals(b.path) - && a.dev == b.dev + // && a.dev == b.dev (dev is not constant in Windows) && a.name.equals(b.name) && a.productId == b.productId && a.vendorId == b.vendorId; @@ -363,4 +444,15 @@ private boolean containsName( return configList.stream() .anyMatch(configuration -> configuration.uniqueName.equals(uniqueName)); } + + /** + * Check if the current list of known cameras contains the given unique name. + * + * @param uniqueName The unique name. + * @return If the list of cameras contains the unique name. + */ + private boolean containsName(final String uniqueName) { + return VisionModuleManager.getInstance().getModules().stream() + .anyMatch(camera -> camera.visionSource.cameraConfiguration.uniqueName.equals(uniqueName)); + } } diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index 2c64a9a622..e16213000e 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -35,23 +35,90 @@ public void visionSourceTest() { ConfigManager.getInstance().load(); inst.tryMatchUSBCamImpl(); - var config = new CameraConfiguration("secondTestVideo", "dev/video1"); + var config3 = + new CameraConfiguration( + "secondTestVideo", + "secondTestVideo1", + "secondTestVideo1", + "dev/video1", + new String[] {"by-id/123"}); + var config4 = + new CameraConfiguration( + "secondTestVideo", + "secondTestVideo2", + "secondTestVideo2", + "dev/video2", + new String[] {"by-id/321"}); + UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); + infoList.add(info1); - inst.registerLoadedConfigs(config); - var sources = inst.tryMatchUSBCamImpl(false); + inst.registerLoadedConfigs(config3, config4); + + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info1)); - assertEquals(1, inst.unmatchedLoadedConfigs.size()); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info2 = new UsbCameraInfo(0, "dev/video1", "testVideo", new String[0], 1, 2); - UsbCameraInfo info2 = - new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 1); infoList.add(info2); + + var cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + // assertEquals("testVideo (1)", cams.get(0).uniqueName); // Proper suffixing + + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info2)); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info3 = + new UsbCameraInfo(0, "dev/video2", "secondTestVideo", new String[] {"by-id/123"}, 2, 1); + + UsbCameraInfo info4 = + new UsbCameraInfo(0, "dev/video3", "secondTestVideo", new String[] {"by-id/321"}, 3, 1); + + infoList.add(info4); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + var cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + // If this is null, cam4 got matched to config3 instead of config4 + + assertEquals(cam4.nickname, config4.nickname); + + infoList.add(info3); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info2)); - assertEquals(2, inst.knownUsbCameras.size()); + assertTrue(inst.knownUsbCameras.contains(info3)); + + var cam3 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config3.otherPaths[0])) + .findFirst() + .orElse(null); + cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + + assertEquals(cam3.nickname, config3.nickname); + assertEquals(cam4.nickname, config4.nickname); + assertEquals(4, inst.knownUsbCameras.size()); assertEquals(0, inst.unmatchedLoadedConfigs.size()); } } From d67b66540744b66ac4921d1e7236413c4d52160d Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 11:44:16 -0500 Subject: [PATCH 11/20] Update upload/download artifact action (#1043) --- .github/workflows/build.yml | 38 ++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7a0365769c..6d329f400c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -16,16 +16,16 @@ jobs: working-directory: photon-client runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Node.js - uses: actions/setup-node@v3 + uses: actions/setup-node@v4 with: node-version: 18 - name: Install Dependencies run: npm ci - name: Build Production Client run: npm run build - - uses: actions/upload-artifact@master + - uses: actions/upload-artifact@v4 with: name: built-client path: photon-client/dist/ @@ -34,13 +34,13 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - name: Fetch tags run: git fetch --tags --force - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -68,7 +68,7 @@ jobs: steps: # Checkout code. - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - name: Fetch tags @@ -98,11 +98,11 @@ jobs: name: "Build Offline Docs" runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: repository: 'PhotonVision/photonvision-docs.git' ref: master - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: '3.9' - name: Install dependencies @@ -136,11 +136,11 @@ jobs: name: "Photonlib - Build Host - ${{ matrix.artifact-name }}" runs-on: ${{ matrix.os }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -169,7 +169,7 @@ jobs: container: ${{ matrix.container }} name: "Photonlib - Build Docker - ${{ matrix.artifact-name }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Config Git @@ -222,11 +222,11 @@ jobs: name: "Build fat JAR - ${{ matrix.artifact-name }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -238,11 +238,11 @@ jobs: del photon-server\src\main\resources\web\*.* mkdir photon-server\src\main\resources\web\docs if: ${{ (matrix.os) == 'windows-latest' }} - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: built-client path: photon-server/src/main/resources/web/ - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: built-docs path: photon-server/src/main/resources/web/docs @@ -254,7 +254,7 @@ jobs: chmod +x gradlew ./gradlew photon-server:shadowJar --max-workers 2 if: ${{ (matrix.arch-override == 'none') }} - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 with: name: jar-${{ matrix.artifact-name }} path: photon-server/build/libs @@ -280,17 +280,17 @@ jobs: steps: - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v4 with: name: jar-${{ matrix.artifact-name }} - name: Generate image run: | chmod +x scripts/generatePiImage.sh ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }} - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 name: Upload image with: name: image-${{ matrix.image_suffix }} From 3de878c510ba0f6f6b4bc7aef294ada31614449b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 13:00:02 -0500 Subject: [PATCH 12/20] update download all artifacts (#1044) * Update build.yml * Update build.yml --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 6d329f400c..58d2356305 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -301,7 +301,7 @@ jobs: steps: # Download literally every single artifact. This also downloads client and docs, # but the filtering below won't pick these up (I hope) - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v4 - run: find # Push to dev release - uses: pyTooling/Actions/releaser@r0 From 2e39549771eced81962d8a7235bcba8832d3d2ca Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 13:14:52 -0500 Subject: [PATCH 13/20] Bump wpilib to 2024-beta-4 and report resource photon usage ids from 2024v2 image (#1042) --- build.gradle | 6 +- .../java/org/photonvision/PhotonCamera.java | 10 +- .../org/photonvision/PhotonPoseEstimator.java | 12 +- .../simulation/SimCameraProperties.java | 126 ++++++++++-------- .../photonvision/estimation/OpenCVHelp.java | 5 +- .../targeting/PhotonPipelineResult.java | 2 +- photonlib-cpp-examples/build.gradle | 2 +- photonlib-java-examples/build.gradle | 2 +- shared/common.gradle | 1 + shared/config.gradle | 2 +- shared/javacommon.gradle | 1 + 11 files changed, 96 insertions(+), 73 deletions(-) diff --git a/build.gradle b/build.gradle index f772c89e32..3c00130fd5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false + id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" id 'edu.wpi.first.WpilibTools' version '1.3.0' } @@ -20,7 +20,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3" + wpilibVersion = "2024.1.1-beta-4" wpimathVersion = wpilibVersion openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 8b46a2c372..faa03ebc54 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,6 +24,8 @@ package org.photonvision; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -52,6 +54,7 @@ /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { + private static int InstanceCount = 0; public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; @@ -152,6 +155,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); + + HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; } /** @@ -321,14 +327,14 @@ public boolean isConnected() { public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); } else return Optional.empty(); } public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); } else return Optional.empty(); } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index dbf36292c2..f93c499140 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -25,6 +25,8 @@ package org.photonvision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -52,6 +54,8 @@ * below. Example usage can be found in our apriltagExample example project. */ public class PhotonPoseEstimator { + private static int InstanceCount = 0; + /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */ public enum PoseStrategy { /** Choose the Pose with the lowest ambiguity. */ @@ -118,14 +122,14 @@ public PhotonPoseEstimator( this.primaryStrategy = strategy; this.camera = camera; this.robotToCamera = robotToCamera; + + HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); + InstanceCount++; } public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = null; - this.robotToCamera = robotToCamera; + this(fieldTags, strategy, null, robotToCamera); } /** Invalidates the pose cache. */ diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 6d157fb252..f7df72487a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,6 +25,7 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -146,8 +147,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException setCalibration( jsonWidth, jsonHeight, - Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), - Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics), + MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion)); setCalibError(jsonAvgError, jsonErrorStdDev); success = true; } @@ -184,7 +185,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); // create camera intrinsics matrix - var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1); setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); } @@ -597,17 +598,19 @@ public static SimCameraProperties PI4_LIFECAM_320_240() { prop.setCalibration( 320, 240, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 328.2733242048587, - 0.0, - 164.8190261141906, - 0.0, - 318.0609794305216, - 123.8633838438093, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.09957946553445934, -0.9166265114485799, @@ -626,17 +629,19 @@ public static SimCameraProperties PI4_LIFECAM_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 669.1428078983059, - 0.0, - 322.53377249329213, - 0.0, - 646.9843137061716, - 241.26567383784163, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.12788470750464645, -1.2350335805796528, @@ -655,17 +660,18 @@ public static SimCameraProperties LL2_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 511.22843367007755, - 0.0, - 323.62049380211096, - 0.0, - 514.5452336723849, - 261.8827920543568, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.1917469998873756, -0.5142936883324216, @@ -684,17 +690,19 @@ public static SimCameraProperties LL2_960_720() { prop.setCalibration( 960, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 769.6873145148892, - 0.0, - 486.1096609458122, - 0.0, - 773.8164483705323, - 384.66071662358354, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.189462064814501, -0.49903003669627627, @@ -713,17 +721,19 @@ public static SimCameraProperties LL2_1280_720() { prop.setCalibration( 1280, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 1011.3749416937393, - 0.0, - 645.4955139388737, - 0.0, - 1008.5391755084075, - 508.32877656020196, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.13730101577061535, -0.2904345656989261, diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ed53a17b80..e2413a4d22 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -18,6 +18,7 @@ package org.photonvision.estimation; import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -63,8 +64,8 @@ public static void forceLoadOpenCV() { } static { - NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); - EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); + NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 9a8ce65db9..7047ee46aa 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -88,7 +88,7 @@ public PhotonTrackedTarget getBestTarget() { String errStr = "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " + "before calling this method. For more information, please review the PhotonLib " - + "documentation at http://docs.photonvision.org"; + + "documentation at https://docs.photonvision.org"; System.err.println(errStr); new Exception().printStackTrace(); HAS_WARNED = true; diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 881e04a114..51263e12bc 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } allprojects { diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 1c92e043f9..fb2207b6c5 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } apply from: "examples.gradle" diff --git a/shared/common.gradle b/shared/common.gradle index faeaa1f694..7cbf858b22 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -32,6 +32,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/shared/config.gradle b/shared/config.gradle index b3c7781c84..4044d7e3ff 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -14,7 +14,7 @@ nativeUtils.wpi.configureDependencies { opencvVersion = openCVversion googleTestYear = "frc2024" googleTestVersion = "1.14.0-1" - niLibVersion = "2024.1.1" + niLibVersion = "2024.2.1" } // Configure warnings and errors diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index c4e0ad3b56..55f72d0f60 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -113,6 +113,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() From 47aea29b6b3e0e1e16d1ed59a51e7799c57e51f0 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sat, 16 Dec 2023 12:32:49 -0600 Subject: [PATCH 14/20] Add photonlibpy (#1040) * Added a pure-python implementation of photonlib, named photonlibpy and hosted on pypi --------- Co-authored-by: Matt --- .github/workflows/python.yml | 62 +++++ photon-lib/py/.gitignore | 5 + photon-lib/py/buildAndTest.bat | 14 + photon-lib/py/photonlibpy/__init__.py | 1 + .../py/photonlibpy/multiTargetPNPResult.py | 45 ++++ photon-lib/py/photonlibpy/packet.py | 143 +++++++++++ photon-lib/py/photonlibpy/photonCamera.py | 170 +++++++++++++ .../py/photonlibpy/photonPipelineResult.py | 38 +++ .../py/photonlibpy/photonTrackedTarget.py | 82 ++++++ photon-lib/py/setup.py | 54 ++++ photon-lib/py/test/data.py | 239 ++++++++++++++++++ photon-lib/py/test/photonlibpy_test.py | 46 ++++ 12 files changed, 899 insertions(+) create mode 100644 .github/workflows/python.yml create mode 100644 photon-lib/py/.gitignore create mode 100644 photon-lib/py/buildAndTest.bat create mode 100644 photon-lib/py/photonlibpy/__init__.py create mode 100644 photon-lib/py/photonlibpy/multiTargetPNPResult.py create mode 100644 photon-lib/py/photonlibpy/packet.py create mode 100644 photon-lib/py/photonlibpy/photonCamera.py create mode 100644 photon-lib/py/photonlibpy/photonPipelineResult.py create mode 100644 photon-lib/py/photonlibpy/photonTrackedTarget.py create mode 100644 photon-lib/py/setup.py create mode 100644 photon-lib/py/test/data.py create mode 100644 photon-lib/py/test/photonlibpy_test.py diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml new file mode 100644 index 0000000000..de54a54756 --- /dev/null +++ b/.github/workflows/python.yml @@ -0,0 +1,62 @@ +name: Build and Distribute PhotonLibPy + +permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing + +on: + push: + branches: [ master ] + tags: + - 'v*' + pull_request: + branches: [ master ] + +jobs: + buildAndDeploy: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + sparse-checkout-cone-mode: false + fetch-tags: true + fetch-depth: 99999 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: 3.11 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install setuptools wheel pytest + + - name: Build wheel + working-directory: ./photon-lib/py + run: | + python setup.py sdist bdist_wheel + + - name: Run Unit Tests + working-directory: ./photon-lib/py + run: | + pip install --no-cache-dir dist/*.whl + pytest + + + - name: Upload artifacts + uses: actions/upload-artifact@master + with: + name: dist + path: ./photon-lib/py/dist/ + + - name: Publish package distributions to TestPyPI + # Only upload on tags + if: startsWith(github.ref, 'refs/tags/v') + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages_dir: ./photon-lib/py/dist/ + + permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing diff --git a/photon-lib/py/.gitignore b/photon-lib/py/.gitignore new file mode 100644 index 0000000000..f0c4ced6d6 --- /dev/null +++ b/photon-lib/py/.gitignore @@ -0,0 +1,5 @@ +photonlibpy.egg-info/ +dist/ +build/ +.eggs/ +photonlibpy/version.py diff --git a/photon-lib/py/buildAndTest.bat b/photon-lib/py/buildAndTest.bat new file mode 100644 index 0000000000..b9774a54d6 --- /dev/null +++ b/photon-lib/py/buildAndTest.bat @@ -0,0 +1,14 @@ +:: Uninstall if it already was installed +pip uninstall -y photonlibpy + +:: Build wheel +python setup.py bdist_wheel + +:: Install whatever wheel was made +for %%f in (dist/*.whl) do ( + echo installing dist/%%f + pip install --no-cache-dir dist/%%f +) + +:: Run the test suite +pytest diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py new file mode 100644 index 0000000000..d5b258f5e7 --- /dev/null +++ b/photon-lib/py/photonlibpy/__init__.py @@ -0,0 +1 @@ +# No one here but us chickens diff --git a/photon-lib/py/photonlibpy/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/multiTargetPNPResult.py new file mode 100644 index 0000000000..63bbb34301 --- /dev/null +++ b/photon-lib/py/photonlibpy/multiTargetPNPResult.py @@ -0,0 +1,45 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class PNPResult: + _NUM_BYTES_IN_FLOAT = 8 + PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3) + + isPresent: bool = False + best: Transform3d = field(default_factory=Transform3d) + alt: Transform3d = field(default_factory=Transform3d) + ambiguity: float = 0.0 + bestReprojError: float = 0.0 + altReprojError: float = 0.0 + + def createFromPacket(self, packet: Packet) -> Packet: + self.isPresent = packet.decodeBoolean() + self.best = packet.decodeTransform() + self.alt = packet.decodeTransform() + self.bestReprojError = packet.decodeDouble() + self.altReprojError = packet.decodeDouble() + self.ambiguity = packet.decodeDouble() + return packet + + +@dataclass +class MultiTargetPNPResult: + _MAX_IDS = 32 + # pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) + _PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS) + + estimatedPose: PNPResult = field(default_factory=PNPResult) + fiducialIDsUsed: list[int] = field(default_factory=list) + + def createFromPacket(self, packet: Packet) -> Packet: + self.estimatedPose = PNPResult() + self.estimatedPose.createFromPacket(packet) + self.fiducialIDsUsed = [] + for _ in range(MultiTargetPNPResult._MAX_IDS): + fidId = packet.decode16() + if fidId >= 0: + self.fiducialIDsUsed.append(fidId) + return packet diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py new file mode 100644 index 0000000000..c3816d94f8 --- /dev/null +++ b/photon-lib/py/photonlibpy/packet.py @@ -0,0 +1,143 @@ +import struct +from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion +import wpilib + + +class Packet: + def __init__(self, data: list[int]): + """ + * Constructs an empty packet. + * + * @param self.size The self.size of the packet buffer. + """ + self.packetData = data + self.size = len(data) + self.readPos = 0 + self.outOfBytes = False + + def clear(self): + """Clears the packet and resets the read and write positions.""" + self.packetData = [0] * self.size + self.readPos = 0 + self.outOfBytes = False + + def getSize(self): + return self.size + + _NO_MORE_BYTES_MESSAGE = """ + Photonlib - Ran out of bytes while decoding. + Make sure the version of photonvision on the coprocessor + matches the version of photonlib running in the robot code. + """ + + def _getNextByte(self) -> int: + retVal = 0x00 + + if not self.outOfBytes: + try: + retVal = 0x00FF & self.packetData[self.readPos] + self.readPos += 1 + except IndexError: + wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True) + self.outOfBytes = True + + return retVal + + def getData(self) -> list[int]: + """ + * Returns the packet data. + * + * @return The packet data. + """ + return self.packetData + + def setData(self, data: list[int]): + """ + * Sets the packet data. + * + * @param data The packet data. + """ + self.clear() + self.packetData = data + self.size = len(self.packetData) + + def _decodeGeneric(self, unpackFormat, numBytes): + # Read ints in from the data buffer + intList = [] + for _ in range(numBytes): + intList.append(self._getNextByte()) + + # Interpret the bytes as a floating point number + value = struct.unpack(unpackFormat, bytes(intList))[0] + + return value + + def decode8(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">b", 1) + + def decode16(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">h", 2) + + def decode32(self) -> int: + """ + * Returns a decoded int (32 bytes) from the packet. + * + * @return A decoded int from the packet. + """ + return self._decodeGeneric(">l", 4) + + def decodeDouble(self) -> float: + """ + * Returns a decoded double from the packet. + * + * @return A decoded double from the packet. + """ + return self._decodeGeneric(">d", 8) + + def decodeBoolean(self) -> bool: + """ + * Returns a decoded boolean from the packet. + * + * @return A decoded boolean from the packet. + """ + return self.decode8() == 1 + + def decodeDoubleArray(self, length: int) -> list[float]: + """ + * Returns a decoded array of floats from the packet. + * + * @return A decoded array of floats from the packet. + """ + ret = [] + for _ in range(length): + ret.append(self.decodeDouble()) + return ret + + def decodeTransform(self) -> Transform3d: + """ + * Returns a decoded Transform3d + * + * @return A decoded Tansform3d from the packet. + """ + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + translation = Translation3d(x, y, z) + + w = self.decodeDouble() + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + rotation = Rotation3d(Quaternion(w, x, y, z)) + + return Transform3d(translation, rotation) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py new file mode 100644 index 0000000000..0d474a2434 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -0,0 +1,170 @@ +from enum import Enum +import ntcore +from wpilib import Timer +import wpilib +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION + + +class VisionLEDMode(Enum): + kDefault = -1 + kOff = 0 + kOn = 1 + kBlink = 2 + + +lastVersionTimeCheck = 0.0 +_VERSION_CHECK_ENABLED = True + + +def setVersionCheckEnabled(enabled: bool): + _VERSION_CHECK_ENABLED = enabled + + +class PhotonCamera: + def __init__(self, cameraName: str): + instance = ntcore.NetworkTableInstance.getDefault() + self.name = cameraName + self._tableName = "photonvision" + photonvision_root_table = instance.getTable(self._tableName) + self.cameraTable = photonvision_root_table.getSubTable(cameraName) + self.path = self.cameraTable.getPath() + self.rawBytesEntry = self.cameraTable.getRawTopic("rawBytes").subscribe( + "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True) + ) + + self.driverModePublisher = self.cameraTable.getBooleanTopic( + "driverModeRequest" + ).publish() + self.driverModeSubscriber = self.cameraTable.getBooleanTopic( + "driverMode" + ).subscribe(False) + self.inputSaveImgEntry = self.cameraTable.getIntegerTopic( + "inputSaveImgCmd" + ).getEntry(0) + self.outputSaveImgEntry = self.cameraTable.getIntegerTopic( + "outputSaveImgCmd" + ).getEntry(0) + self.pipelineIndexRequest = self.cameraTable.getIntegerTopic( + "pipelineIndexRequest" + ).publish() + self.pipelineIndexState = self.cameraTable.getIntegerTopic( + "pipelineIndexState" + ).subscribe(0) + self.heartbeatEntry = self.cameraTable.getIntegerTopic("heartbeat").subscribe( + -1 + ) + + self.ledModeRequest = photonvision_root_table.getIntegerTopic( + "ledModeRequest" + ).publish() + self.ledModeState = photonvision_root_table.getIntegerTopic( + "ledModeState" + ).subscribe(-1) + self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe( + "" + ) + + # Existing is enough to make this multisubscriber do its thing + self.topicNameSubscriber = ntcore.MultiSubscriber( + instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True) + ) + + self.prevHeartbeat = 0 + self.prevHeartbeatChangeTime = Timer.getFPGATimestamp() + + def getLatestResult(self) -> PhotonPipelineResult: + self._versionCheck() + + retVal = PhotonPipelineResult() + packetWithTimestamp = self.rawBytesEntry.getAtomic() + byteList = packetWithTimestamp.value + timestamp = packetWithTimestamp.time + + if len(byteList) < 1: + return retVal + else: + retVal.populateFromPacket(Packet(byteList)) + # NT4 allows us to correct the timestamp based on when the message was sent + retVal.setTimestampSeconds( + timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3 + ) + return retVal + + def getDriverMode(self) -> bool: + return self.driverModeSubscriber.get() + + def setDriverMode(self, driverMode: bool) -> None: + self.driverModePublisher.set(driverMode) + + def takeInputSnapshot(self) -> None: + self.inputSaveImgEntry.set(self.inputSaveImgEntry.get() + 1) + + def takeOutputSnapshot(self) -> None: + self.outputSaveImgEntry.set(self.outputSaveImgEntry.get() + 1) + + def getPipelineIndex(self) -> int: + return self.pipelineIndexState.get(0) + + def setPipelineIndex(self, index: int) -> None: + self.pipelineIndexRequest.set(index) + + def getLEDMode(self) -> VisionLEDMode: + mode = self.ledModeState.get() + return VisionLEDMode(mode) + + def setLEDMode(self, led: VisionLEDMode) -> None: + self.ledModeRequest.set(led.value) + + def getName(self) -> str: + return self.name + + def isConnected(self) -> bool: + curHeartbeat = self.heartbeatEntry.get() + now = Timer.getFPGATimestamp() + + if curHeartbeat != self.prevHeartbeat: + self.prevHeartbeat = curHeartbeat + self.prevHeartbeatChangeTime = now + + return (now - self.prevHeartbeatChangeTime) < 0.5 + + def _versionCheck(self) -> None: + if not _VERSION_CHECK_ENABLED: + return + + if (Timer.getFPGATimestamp() - lastVersionTimeCheck) < 5.0: + return + + if not self.heartbeatEntry.exists(): + cameraNames = ( + self.cameraTable.getInstance().getTable(self._tableName).getSubTables() + ) + if len(cameraNames) == 0: + wpilib.reportError( + "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", + False, + ) + else: + wpilib.reportError( + f"PhotonVision coprocessor at path {self.path} not found in Network Tables. Double check that your camera names match! Only the following camera names were found: { ''.join(cameraNames)}", + True, + ) + + elif not self.isConnected(): + wpilib.reportWarning( + f"PhotonVision coprocessor at path {self.path} is not sending new data.", + True, + ) + + versionString = self.versionEntry.get(defaultValue="") + if len(versionString) > 0 and versionString != PHOTONVISION_VERSION: + wpilib.reportWarning( + "Photon version " + + PHOTONVISION_VERSION + + " does not match coprocessor version " + + versionString + + f"! Please install photonlibpy version {PHOTONLIB_VERSION}", + True, + ) diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py new file mode 100644 index 0000000000..3fe02c9fea --- /dev/null +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -0,0 +1,38 @@ +from dataclasses import dataclass, field + +from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult +from photonlibpy.packet import Packet +from photonlibpy.photonTrackedTarget import PhotonTrackedTarget + + +@dataclass +class PhotonPipelineResult: + latencyMillis: float = -1.0 + timestampSec: float = -1.0 + targets: list[PhotonTrackedTarget] = field(default_factory=list) + multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult) + + def populateFromPacket(self, packet: Packet) -> Packet: + self.targets = [] + self.latencyMillis = packet.decodeDouble() + self.multiTagResult = MultiTargetPNPResult() + self.multiTagResult.createFromPacket(packet) + targetCount = packet.decode8() + for _ in range(targetCount): + target = PhotonTrackedTarget() + target.createFromPacket(packet) + self.targets.append(target) + + return packet + + def setTimestampSeconds(self, timestampSec: float) -> None: + self.timestampSec = timestampSec + + def getLatencyMillis(self) -> float: + return self.latencyMillis + + def getTimestamp(self) -> float: + return self.timestampSec + + def getTargets(self) -> list[PhotonTrackedTarget]: + return self.targets diff --git a/photon-lib/py/photonlibpy/photonTrackedTarget.py b/photon-lib/py/photonlibpy/photonTrackedTarget.py new file mode 100644 index 0000000000..56c225d997 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonTrackedTarget.py @@ -0,0 +1,82 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class TargetCorner: + x: float + y: float + + +@dataclass +class PhotonTrackedTarget: + _MAX_CORNERS = 8 + _NUM_BYTES_IN_FLOAT = 8 + _PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS) + + yaw: float = 0.0 + pitch: float = 0.0 + area: float = 0.0 + skew: float = 0.0 + fiducialId: int = -1 + bestCameraToTarget: Transform3d = field(default_factory=Transform3d) + altCameraToTarget: Transform3d = field(default_factory=Transform3d) + minAreaRectCorners: list[TargetCorner] | None = None + detectedCorners: list[TargetCorner] | None = None + poseAmbiguity: float = 0.0 + + def getYaw(self) -> float: + return self.yaw + + def getPitch(self) -> float: + return self.pitch + + def getArea(self) -> float: + return self.area + + def getSkew(self) -> float: + return self.skew + + def getFiducialId(self) -> int: + return self.fiducialId + + def getPoseAmbiguity(self) -> float: + return self.poseAmbiguity + + def getMinAreaRectCorners(self) -> list[TargetCorner] | None: + return self.minAreaRectCorners + + def getDetectedCorners(self) -> list[TargetCorner] | None: + return self.detectedCorners + + def getBestCameraToTarget(self) -> Transform3d: + return self.bestCameraToTarget + + def getAlternateCameraToTarget(self) -> Transform3d: + return self.altCameraToTarget + + def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]: + retList = [] + for _ in range(numTargets): + cx = packet.decodeDouble() + cy = packet.decodeDouble() + retList.append(TargetCorner(cx, cy)) + return retList + + def createFromPacket(self, packet: Packet) -> Packet: + self.yaw = packet.decodeDouble() + self.pitch = packet.decodeDouble() + self.area = packet.decodeDouble() + self.skew = packet.decodeDouble() + self.fiducialId = packet.decode32() + + self.bestCameraToTarget = packet.decodeTransform() + self.altCameraToTarget = packet.decodeTransform() + + self.poseAmbiguity = packet.decodeDouble() + + self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four + numCorners = packet.decode8() + self.detectedCorners = self._decodeTargetList(packet, numCorners) + return packet diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py new file mode 100644 index 0000000000..b1c3a06133 --- /dev/null +++ b/photon-lib/py/setup.py @@ -0,0 +1,54 @@ +from setuptools import setup, find_packages +import subprocess, re + +gitDescribeResult = ( + subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"]) + .decode("utf-8") + .strip() +) + +m = re.search( + r"(v[0-9]{4}\.[0-9]{1}\.[0-9]{1})-?((?:beta)?(?:alpha)?)-?([0-9\.]*)", + gitDescribeResult, +) + +# Extract the first portion of the git describe result +# which should be PEP440 compliant +if m: + versionString = m.group(0) + prefix = m.group(1) + maturity = m.group(2) + suffix = m.group(3).replace(".", "") + versionString = f"{prefix}.{maturity}.{suffix}" + + +else: + print("Warning, no valid version found") + versionString = gitDescribeResult + +print(f"Building version {versionString}") + +# Put the version info into a python file for runtime access +with open("photonlibpy/version.py", "w", encoding="utf-8") as fp: + fp.write(f'PHOTONLIB_VERSION="{versionString}"\n') + fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n') + + +descriptionStr = f""" +Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. +Implemented with PhotonVision version {gitDescribeResult} . +""" + +setup( + name="photonlibpy", + packages=find_packages(), + version=versionString, + install_requires=[ + "wpilib<2025,>=2024.0.0b2", + "robotpy-wpimath<2025,>=2024.0.0b2", + "pyntcore<2025,>=2024.0.0b2", + ], + description=descriptionStr, + url="https://photonvision.org", + author="Photonvision Development Team", +) diff --git a/photon-lib/py/test/data.py b/photon-lib/py/test/data.py new file mode 100644 index 0000000000..8c317dfe2f --- /dev/null +++ b/photon-lib/py/test/data.py @@ -0,0 +1,239 @@ +# fmt: off +rawBytes1 = [ + 64, 166, 117, 41, 225, 243, 165, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes2 = [ + 64, 114, 72, 58, 227, 96, 141, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 0 +] +rawBytes3 = [ + 64, 55, 65, 189, 215, 102, 131, 195, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes4 = [ + 64, 115, 23, 245, 248, 9, 145, 121, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 41, 32, 212, 70, 53, 253, 38, 64, 19, 140, + 198, 187, 206, 56, 251, 64, 38, 63, 170, 170, 170, 170, 170, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 17, 63, 226, 15, 19, 123, 229, 152, 236, 191, 191, 225, + 27, 96, 1, 219, 30, 63, 168, 175, 103, 65, 172, 24, 8, 63, 115, 51, 123, + 216, 202, 14, 128, 191, 155, 163, 119, 215, 217, 209, 224, 63, 212, 76, 79, + 227, 166, 197, 80, 63, 238, 85, 211, 252, 64, 132, 136, 63, 226, 13, 126, + 170, 219, 202, 209, 191, 190, 134, 80, 94, 25, 179, 17, 63, 168, 92, 53, + 102, 36, 28, 64, 63, 204, 203, 52, 12, 186, 226, 51, 63, 148, 67, 104, 89, + 131, 114, 208, 63, 211, 104, 18, 25, 149, 138, 78, 63, 237, 159, 242, 53, + 211, 204, 51, 63, 217, 254, 169, 82, 190, 36, 22, 64, 123, 96, 0, 6, 100, + 53, 178, 64, 112, 207, 255, 241, 198, 25, 18, 64, 132, 215, 255, 254, 189, + 61, 109, 64, 86, 191, 255, 217, 164, 214, 161, 64, 138, 55, 94, 60, 205, + 229, 39, 64, 115, 130, 222, 78, 57, 230, 238, 64, 131, 15, 94, 65, 66, 194, + 147, 64, 126, 162, 222, 73, 150, 202, 88, 4, 64, 130, 248, 64, 192, 0, 0, 0, + 64, 126, 15, 133, 64, 0, 0, 0, 64, 137, 206, 237, 128, 0, 0, 0, 64, 116, 48, + 240, 32, 0, 0, 0, 64, 132, 218, 43, 96, 0, 0, 0, 64, 86, 210, 155, 128, 0, + 0, 0, 64, 123, 102, 127, 192, 0, 0, 0, 64, 112, 211, 233, 96, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] +rawBytes5 = [ + 64, 102, 149, 235, 181, 90, 192, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 21, 210, 112, 148, 86, 4, 131, 64, 3, 87, + 196, 18, 174, 105, 145, 64, 47, 80, 237, 9, 123, 66, 95, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 17, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, + 0, 0, 0, 0, 64, 116, 192, 0, 0, 0, 0, 0, 64, 127, 128, 0, 0, 0, 0, 0, 64, + 116, 192, 0, 0, 0, 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, + 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, 0, 0, 64, 127, 128, + 0, 0, 0, 0, 0, 4, 64, 133, 78, 45, 224, 0, 0, 0, 64, 127, 129, 184, 160, 0, + 0, 0, 64, 133, 78, 1, 192, 0, 0, 0, 64, 100, 100, 194, 224, 0, 0, 0, 64, + 118, 181, 224, 64, 0, 0, 0, 64, 102, 98, 136, 0, 0, 0, 0, 64, 116, 207, 155, + 64, 0, 0, 0, 64, 126, 121, 100, 96, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0 +] +rawBytes6 = [ + 64, 78, 129, 235, 32, 116, 234, 142, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 8, 64, 43, 213, 73, 8, 241, 221, 240, 192, 2, 146, + 71, 190, 201, 205, 25, 64, 37, 96, 141, 183, 156, 102, 0, 192, 9, 112, 76, + 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 101, 10, 36, 92, 234, 132, 108, 64, 89, 45, 24, + 60, 197, 216, 218, 64, 114, 7, 35, 134, 17, 204, 20, 64, 87, 129, 247, 219, + 201, 12, 226, 64, 114, 67, 177, 81, 138, 189, 202, 64, 100, 68, 236, 33, + 157, 19, 147, 64, 101, 131, 63, 243, 220, 103, 216, 64, 101, 26, 124, 82, + 27, 121, 143, 0, 192, 32, 131, 181, 181, 155, 145, 13, 192, 37, 92, 235, 61, + 221, 83, 253, 63, 173, 176, 233, 61, 133, 212, 255, 192, 73, 171, 139, 128, + 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 92, 42, 37, 190, 203, 146, 101, 64, 102, 61, 168, + 153, 137, 73, 186, 64, 94, 48, 100, 45, 88, 178, 51, 64, 100, 249, 193, 146, + 50, 77, 24, 64, 94, 220, 25, 65, 52, 109, 155, 64, 101, 62, 112, 102, 118, + 182, 70, 64, 92, 213, 218, 210, 167, 77, 205, 64, 102, 130, 87, 109, 205, + 178, 232, 0, 192, 19, 156, 59, 67, 88, 173, 35, 192, 37, 85, 193, 76, 240, + 22, 41, 63, 152, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, + 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, + 96, 128, 0, 0, 0, 0, 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, + 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, 0, 64, 101, 224, 0, + 0, 0, 0, 0, 64, 96, 128, 0, 0, 0, 0, 0, 64, 101, 224, 0, 0, 0, 0, 0, 0, 64, + 48, 164, 171, 25, 83, 4, 154, 192, 34, 178, 85, 3, 174, 51, 62, 63, 147, + 255, 255, 245, 245, 4, 85, 192, 28, 128, 4, 0, 0, 0, 0, 255, 255, 255, 255, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, + 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 110, + 24, 157, 164, 237, 183, 149, 64, 101, 4, 236, 103, 73, 0, 224, 64, 111, 24, + 157, 165, 1, 191, 69, 64, 100, 228, 236, 103, 153, 126, 38, 64, 111, 32, 0, + 27, 18, 72, 107, 64, 101, 32, 0, 24, 182, 255, 32, 64, 110, 32, 0, 26, 254, + 64, 187, 64, 101, 64, 0, 24, 102, 129, 218, 0, 63, 247, 149, 178, 38, 100, + 246, 86, 192, 9, 184, 134, 230, 194, 222, 110, 63, 150, 102, 102, 85, 138, + 188, 43, 64, 81, 228, 41, 192, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 100, 147, 51, 63, + 205, 146, 189, 64, 96, 153, 153, 160, 125, 112, 225, 64, 100, 224, 0, 12, + 123, 135, 101, 64, 96, 128, 0, 6, 164, 45, 139, 64, 101, 35, 51, 64, 50, + 109, 67, 64, 97, 73, 153, 159, 130, 143, 31, 64, 100, 214, 102, 115, 132, + 120, 155, 64, 97, 99, 51, 57, 91, 210, 117, 0, 192, 33, 131, 192, 140, 90, + 220, 117, 192, 54, 53, 227, 123, 209, 81, 175, 63, 136, 0, 0, 0, 0, 0, 0, + 64, 86, 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 92, 128, 0, 0, 0, 0, 0, 64, + 109, 128, 0, 0, 0, 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 128, 0, 0, 0, + 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 64, 92, 128, + 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 0, 192, 25, 145, 202, 146, 13, + 244, 248, 192, 36, 180, 8, 18, 34, 149, 42, 63, 128, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, 64, 101, 96, 0, + 0, 0, 0, 0, 64, 96, 32, 0, 0, 0, 0, 0, 64, 101, 96, 0, 0, 0, 0, 0, 64, 96, + 32, 0, 0, 0, 0, 0, 64, 101, 160, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, + 64, 101, 160, 0, 0, 0, 0, 0, 0, 192, 35, 144, 241, 16, 205, 7, 236, 192, 0, + 254, 21, 96, 184, 112, 69, 63, 135, 255, 255, 219, 151, 33, 85, 192, 70, + 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 91, 0, 0, 64, 109, 58, 156, 64, 96, + 96, 0, 31, 237, 203, 144, 64, 91, 192, 0, 63, 219, 151, 34, 64, 96, 0, 0, + 32, 54, 157, 78, 64, 92, 32, 0, 63, 146, 197, 100, 64, 96, 48, 0, 32, 18, + 52, 112, 64, 91, 96, 0, 64, 36, 104, 222, 64, 96, 144, 0, 31, 201, 98, 178, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py new file mode 100644 index 0000000000..92b1636351 --- /dev/null +++ b/photon-lib/py/test/photonlibpy_test.py @@ -0,0 +1,46 @@ +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from data import rawBytes1 +from data import rawBytes2 +from data import rawBytes3 +from data import rawBytes4 +from data import rawBytes5 +from data import rawBytes6 + + +def setupCommon(bytesIn): + res = PhotonPipelineResult() + packet = Packet(bytesIn) + res.populateFromPacket(packet) + assert packet.outOfBytes is False + return res + + +def test_byteParse1(): + res = setupCommon(rawBytes1) + assert len(res.getTargets()) == 0 + + +def test_byteParse2(): + res = setupCommon(rawBytes2) + assert len(res.getTargets()) == 0 + + +def test_byteParse3(): + res = setupCommon(rawBytes3) + assert len(res.getTargets()) == 0 + + +def test_byteParse4(): + res = setupCommon(rawBytes4) + assert len(res.getTargets()) == 1 + + +def test_byteParse5(): + res = setupCommon(rawBytes5) + assert len(res.getTargets()) == 1 + + +def test_byteParse6(): + res = setupCommon(rawBytes6) + assert len(res.getTargets()) > 6 From cba4db0bceb144cd4f178131aa09a023bf17dc1a Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Sat, 16 Dec 2023 13:41:27 -0500 Subject: [PATCH 15/20] Update C++ Simulation to match Java (#1026) --- .../main/native/cpp/photon/PhotonCamera.cpp | 17 +- .../main/native/include/photon/PhotonCamera.h | 8 +- .../photon/simulation/PhotonCameraSim.h | 449 +++++++++++++++++ .../photon/simulation/SimCameraProperties.h | 475 ++++++++++++++++++ .../include/photon/simulation/VideoSimUtil.h | 432 ++++++++++++++++ .../photon/simulation/VisionSystemSim.h | 282 +++++++++++ .../photon/simulation/VisionTargetSim.h | 73 +++ .../test/native/cpp/VisionSystemSimTest.cpp | 461 +++++++++++++++++ .../photon/estimation/CameraTargetRelation.h | 61 +++ .../include/photon/estimation/OpenCVHelp.h | 291 +++++++++++ .../photon/estimation/RotTrlTransform3d.h | 102 ++++ .../include/photon/estimation/TargetModel.h | 115 +++++ .../photon/estimation/VisionEstimation.h | 121 +++++ .../include/photon/networktables/NTTopicSet.h | 100 ++++ .../include/photon/targeting/PNPResult.h | 12 +- .../swervedriveposeestsim/.gitignore | 1 + .../.wpilib/wpilib_preferences.json | 6 + .../swervedriveposeestsim/WPILib-License.md | 24 + .../swervedriveposeestsim/build.gradle | 121 +++++ .../swervedriveposeestsim/gradlew | 241 +++++++++ .../swervedriveposeestsim/gradlew.bat | 91 ++++ .../swervedriveposeestsim/networktables.json | 1 + .../swervedriveposeestsim/settings.gradle | 30 ++ .../swervedriveposeestsim/simgui-ds.json | 98 ++++ .../swervedriveposeestsim/simgui-window.json | 64 +++ .../swervedriveposeestsim/simgui.json | 57 +++ .../src/main/cpp/Robot.cpp | 123 +++++ .../src/main/cpp/subsystems/SwerveDrive.cpp | 211 ++++++++ .../main/cpp/subsystems/SwerveDriveSim.cpp | 285 +++++++++++ .../src/main/cpp/subsystems/SwerveModule.cpp | 146 ++++++ .../src/main/deploy/example.txt | 4 + .../src/main/include/Constants.h | 118 +++++ .../src/main/include/Robot.h | 62 +++ .../src/main/include/Vision.h | 152 ++++++ .../src/main/include/subsystems/SwerveDrive.h | 85 ++++ .../main/include/subsystems/SwerveDriveSim.h | 102 ++++ .../main/include/subsystems/SwerveModule.h | 81 +++ .../src/test/cpp/main.cpp | 34 ++ .../swervedriveposeestsim/swerve_module.png | Bin 0 -> 2954 bytes .../swervedriveposeestsim/tag-blue.png | Bin 0 -> 4711 bytes .../swervedriveposeestsim/tag-green.png | Bin 0 -> 4698 bytes shared/javacpp/setupBuild.gradle | 2 + 42 files changed, 5123 insertions(+), 15 deletions(-) create mode 100644 photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h create mode 100644 photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp create mode 100644 photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/TargetModel.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h create mode 100644 photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.gitignore create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/build.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/networktables.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/settings.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-green.png diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 95eecb444c..ff26f74fb8 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include "PhotonVersion.h" @@ -128,8 +129,13 @@ LEDMode PhotonCamera::GetLEDMode() const { std::optional PhotonCamera::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { - // clone should deal with ownership concerns? not sure - return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone(); + cv::Mat retVal(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + retVal.at(i, j) = camCoeffs[(j * 3) + i]; + } + } + return retVal; } return std::nullopt; } @@ -145,8 +151,11 @@ const std::string_view PhotonCamera::GetCameraName() const { std::optional PhotonCamera::GetDistCoeffs() { auto distCoeffs = cameraDistortionSubscriber.Get(); if (distCoeffs.size() == 5) { - // clone should deal with ownership concerns? not sure - return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone(); + cv::Mat retVal(5, 1, CV_64FC1); + for (int i = 0; i < 5; i++) { + retVal.at(i, 0) = distCoeffs[i]; + } + return retVal; } return std::nullopt; } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 2a1418f1b5..582458e014 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -39,11 +39,7 @@ #include #include -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting//PhotonPipelineResult.h" namespace cv { class Mat; @@ -172,6 +168,8 @@ class PhotonCamera { PhotonCamera::VERSION_CHECK_ENABLED = enabled; } + std::shared_ptr GetCameraTable() const { return rootTable; } + // For use in tests bool test = false; PhotonPipelineResult testResult; diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h new file mode 100644 index 0000000000..367b30bb8f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -0,0 +1,449 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace photon { +class PhotonCameraSim { + public: + explicit PhotonCameraSim(PhotonCamera* camera) + : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {} + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props) + : prop(props), cam(camera) { + SetMinTargetAreaPixels(kDefaultMinAreaPx); + videoSimRaw = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), + prop.GetResHeight()); + videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray); + videoSimProcessed = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), + prop.GetResHeight()); + ts.subTable = cam->GetCameraTable(); + ts.UpdateEntries(); + } + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, + double minTargetAreaPercent, units::meter_t maxSightRange) + : PhotonCameraSim(camera, props) { + this->minTargetAreaPercent = minTargetAreaPercent; + this->maxSightRange = maxSightRange; + } + PhotonCamera* GetCamera() { return cam; } + double GetMinTargetAreaPercent() { return minTargetAreaPercent; } + double GetMinTargetAreaPixels() { + return minTargetAreaPercent / 100.0 * prop.GetResArea(); + } + units::meter_t GetMaxSightRange() { return maxSightRange; } + const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } + const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; } + bool CanSeeTargetPose(const frc::Pose3d& camPose, + const VisionTargetSim& target) { + CameraTargetRelation rel{camPose, target.GetPose()}; + return ((units::math::abs(rel.camToTargYaw.Degrees()) < + prop.GetHorizFOV().Degrees() / 2.0) && + (units::math::abs(rel.camToTargPitch.Degrees()) < + prop.GetVertFOV().Degrees() / 2.0) && + (!target.GetModel().GetIsPlanar() || + units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && + (rel.camToTarg.Translation().Norm() <= maxSightRange)); + } + bool CanSeeCorner(const std::vector& points) { + for (const auto& pt : points) { + if (std::clamp(pt.x, 0, prop.GetResWidth()) != pt.x || + std::clamp(pt.y, 0, prop.GetResHeight()) != pt.y) { + return false; + } + } + return true; + } + std::optional ConsumeNextEntryTime() { + uint64_t now = wpi::Now(); + uint64_t timestamp{}; + int iter = 0; + while (now >= nextNTEntryTime) { + timestamp = nextNTEntryTime; + uint64_t frameTime = prop.EstSecUntilNextFrame() + .convert() + .to(); + nextNTEntryTime += frameTime; + + if (iter++ > 50) { + timestamp = now; + nextNTEntryTime = now + frameTime; + break; + } + } + + if (timestamp != 0) { + return timestamp; + } else { + return std::nullopt; + } + } + void SetMinTargetAreaPercent(double areaPercent) { + minTargetAreaPercent = areaPercent; + } + void SetMinTargetAreaPixels(double areaPx) { + minTargetAreaPercent = areaPx / prop.GetResArea() * 100; + } + void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; } + void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; } + void SetWireframeResolution(double resolution) { + videoSimWireframeResolution = resolution; + } + void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } + PhotonPipelineResult Process(units::second_t latency, + const frc::Pose3d& cameraPose, + std::vector targets) { + std::sort( + targets.begin(), targets.end(), + [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { + units::meter_t dist1 = + t1.GetPose().Translation().Distance(cameraPose.Translation()); + units::meter_t dist2 = + t2.GetPose().Translation().Distance(cameraPose.Translation()); + return dist1 > dist2; + }); + + std::vector>> + visibleTgts{}; + std::vector detectableTgts{}; + RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose); + + VideoSimUtil::UpdateVideoProp(videoSimRaw, prop); + VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop); + cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()}; + cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1); + blankFrame.assignTo(videoSimFrameRaw); + + for (const auto& tgt : targets) { + if (!CanSeeTargetPose(cameraPose, tgt)) { + continue; + } + + std::vector fieldCorners = tgt.GetFieldVertices(); + if (tgt.GetModel().GetIsSpherical()) { + TargetModel model = tgt.GetModel(); + fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose( + tgt.GetPose().Translation(), cameraPose.Translation())); + } + + std::vector imagePoints = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners); + if (tgt.GetModel().GetIsSpherical()) { + cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints); + int l = 0; + int t = 0; + int b = 0; + int r = 0; + for (int i = 0; i < 4; i++) { + if (imagePoints[i].x < imagePoints[l].x) { + l = i; + } + } + cv::Point2d lc = imagePoints[l]; + std::array angles{}; + t = (l + 1) % 4; + b = (l + 1) % 4; + for (int i = 0; i < 4; i++) { + if (i == l) { + continue; + } + cv::Point2d ic = imagePoints[i]; + angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x); + if (angles[i] >= angles[t]) { + t = i; + } + if (angles[i] <= angles[b]) { + b = i; + } + } + for (int i = 0; i < 4; i++) { + if (i != t && i != l && i != b) { + r = i; + } + } + cv::RotatedRect rect{ + cv::Point2d{center.x, center.y}, + cv::Size2d{imagePoints[r].x - lc.x, + imagePoints[b].y - imagePoints[t].y}, + units::radian_t{-angles[r]}.convert().to()}; + std::vector points{}; + rect.points(points); + + // Can't find an easier way to convert from Point2f to Point2d + imagePoints.clear(); + std::transform(points.begin(), points.end(), + std::back_inserter(imagePoints), + [](const cv::Point2f& p) { return (cv::Point2d)p; }); + } + + visibleTgts.emplace_back(std::make_pair(tgt, imagePoints)); + std::vector noisyTargetCorners = + prop.EstPixelNoise(imagePoints); + cv::RotatedRect minAreaRect = + OpenCVHelp::GetMinAreaRect(noisyTargetCorners); + std::vector minAreaRectPts; + minAreaRectPts.reserve(4); + minAreaRect.points(minAreaRectPts); + cv::Point2d centerPt = minAreaRect.center; + frc::Rotation3d centerRot = prop.GetPixelRot(centerPt); + double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners); + + if (!(CanSeeCorner(noisyTargetCorners) && + areaPercent >= minTargetAreaPercent)) { + continue; + } + + PNPResult pnpSim{}; + if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { + pnpSim = OpenCVHelp::SolvePNP_Square( + prop.GetIntrinsics(), prop.GetDistCoeffs(), + tgt.GetModel().GetVertices(), noisyTargetCorners); + } + + std::vector> tempCorners = + OpenCVHelp::PointsToCorners(minAreaRectPts); + wpi::SmallVector, 4> smallVec; + + for (const auto& corner : tempCorners) { + smallVec.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + std::vector> cornersFloat = + OpenCVHelp::PointsToCorners(noisyTargetCorners); + + std::vector> cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; + detectableTgts.emplace_back(PhotonTrackedTarget{ + centerRot.Z().convert().to(), + -centerRot.Y().convert().to(), areaPercent, + centerRot.X().convert().to(), tgt.fiducialId, + pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble}); + } + + if (videoSimRawEnabled) { + if (videoSimWireframeEnabled) { + VideoSimUtil::DrawFieldWireFrame( + camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1, + cv::Scalar{30}, videoSimFrameRaw); + } + + for (const auto& pair : visibleTgts) { + VisionTargetSim tgt = pair.first; + std::vector corners = pair.second; + + if (tgt.fiducialId > 0) { + VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true, + videoSimFrameRaw); + } else if (!tgt.GetModel().GetIsSpherical()) { + std::vector contour = corners; + if (!tgt.GetModel().GetIsPlanar()) { + contour = OpenCVHelp::GetConvexHull(contour); + } + VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true, + videoSimFrameRaw); + } else { + VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255}, + videoSimFrameRaw); + } + } + videoSimRaw.PutFrame(videoSimFrameRaw); + } else { + videoSimRaw.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + if (videoSimProcEnabled) { + cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, + cv::COLOR_GRAY2BGR); + cv::drawMarker( + videoSimFrameProcessed, + cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0}, + cv::Scalar{0, 255, 0}, cv::MARKER_CROSS, + static_cast( + VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::LINE_AA); + for (const auto& tgt : detectableTgts) { + auto detectedCornersDouble = tgt.GetDetectedCorners(); + std::vector> detectedCornerFloat{ + detectedCornersDouble.begin(), detectedCornersDouble.end()}; + if (tgt.GetFiducialId() >= 0) { + VideoSimUtil::DrawTagDetection( + tgt.GetFiducialId(), + OpenCVHelp::CornersToPoints(detectedCornerFloat), + videoSimFrameProcessed); + } else { + cv::rectangle(videoSimFrameProcessed, + OpenCVHelp::GetBoundingRect( + OpenCVHelp::CornersToPoints(detectedCornerFloat)), + cv::Scalar{0, 0, 255}, + static_cast(VideoSimUtil::GetScaledThickness( + 1, videoSimFrameProcessed)), + cv::LINE_AA); + + wpi::SmallVector, 4> smallVec = + tgt.GetMinAreaRectCorners(); + + std::vector> cornersCopy{}; + cornersCopy.reserve(4); + + for (const auto& corner : smallVec) { + cornersCopy.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + VideoSimUtil::DrawPoly( + OpenCVHelp::CornersToPoints(cornersCopy), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed); + } + } + videoSimProcessed.PutFrame(videoSimFrameProcessed); + } else { + videoSimProcessed.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + MultiTargetPNPResult multiTagResults{}; + + std::vector visibleLayoutTags = + VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); + if (visibleLayoutTags.size() > 1) { + wpi::SmallVector usedIds{}; + std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), + usedIds.begin(), + [](const frc::AprilTag& tag) { return tag.ID; }); + std::sort(usedIds.begin(), usedIds.end()); + PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP( + prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, + kAprilTag16h5); + multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; + } + + return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + } + void SubmitProcessedFrame(const PhotonPipelineResult& result) { + SubmitProcessedFrame(result, wpi::Now()); + } + void SubmitProcessedFrame(const PhotonPipelineResult& result, + uint64_t recieveTimestamp) { + ts.latencyMillisEntry.Set( + result.GetLatency().convert().to(), + recieveTimestamp); + + Packet newPacket{}; + newPacket << result; + ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); + + bool hasTargets = result.HasTargets(); + ts.hasTargetEntry.Set(hasTargets, recieveTimestamp); + if (!hasTargets) { + ts.targetPitchEntry.Set(0.0, recieveTimestamp); + ts.targetYawEntry.Set(0.0, recieveTimestamp); + ts.targetAreaEntry.Set(0.0, recieveTimestamp); + std::array poseData{0.0, 0.0, 0.0}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetSkewEntry.Set(0.0, recieveTimestamp); + } else { + PhotonTrackedTarget bestTarget = result.GetBestTarget(); + + ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp); + ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp); + ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp); + ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp); + + frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); + std::array poseData{ + transform.X().to(), transform.Y().to(), + transform.Rotation().ToRotation2d().Degrees().to()}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + } + + auto intrinsics = prop.GetIntrinsics(); + std::vector intrinsicsView{intrinsics.data(), + intrinsics.data() + intrinsics.size()}; + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + + auto distortion = prop.GetDistCoeffs(); + std::vector distortionView{distortion.data(), + distortion.data() + distortion.size()}; + ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp); + + ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp); + } + SimCameraProperties prop; + + private: + PhotonCamera* cam; + + NTTopicSet ts{}; + uint64_t heartbeatCounter{0}; + + uint64_t nextNTEntryTime{wpi::Now()}; + + units::meter_t maxSightRange{std::numeric_limits::max()}; + static constexpr double kDefaultMinAreaPx{100}; + double minTargetAreaPercent; + + frc::AprilTagFieldLayout tagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + + cs::CvSource videoSimRaw; + cv::Mat videoSimFrameRaw{}; + bool videoSimRawEnabled{true}; + bool videoSimWireframeEnabled{false}; + double videoSimWireframeResolution{0.1}; + cs::CvSource videoSimProcessed; + cv::Mat videoSimFrameProcessed{}; + bool videoSimProcEnabled{true}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h new file mode 100644 index 0000000000..f3a5d73677 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -0,0 +1,475 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace photon { +class SimCameraProperties { + public: + SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + SimCameraProperties(std::string path, int width, int height) {} + void SetCalibration(int width, int height, frc::Rotation2d fovDiag) { + if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) { + fovDiag = frc::Rotation2d{ + std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; + FRC_ReportError( + frc::err::Error, + "Requested invalid FOV! Clamping between (1, 179) degrees..."); + } + double resDiag = std::sqrt(width * width + height * height); + double diagRatio = units::math::tan(fovDiag.Radians() / 2.0); + frc::Rotation2d fovWidth{ + units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; + frc::Rotation2d fovHeight{ + units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; + + Eigen::Matrix newDistCoeffs; + newDistCoeffs << 0, 0, 0, 0, 0; + + double cx = width / 2.0 - 0.5; + double cy = height / 2.0 - 0.5; + + double fx = cx / units::math::tan(fovWidth.Radians() / 2.0); + double fy = cy / units::math::tan(fovHeight.Radians() / 2.0); + + Eigen::Matrix newCamIntrinsics; + newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; + SetCalibration(width, height, newCamIntrinsics, newDistCoeffs); + } + + void SetCalibration(int width, int height, + const Eigen::Matrix& newCamIntrinsics, + const Eigen::Matrix& newDistCoeffs) { + resWidth = width; + resHeight = height; + camIntrinsics = newCamIntrinsics; + distCoeffs = newDistCoeffs; + + std::array p{ + frc::Translation3d{ + 1_m, + frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(0) + frc::Rotation2d{units::radian_t{ + -std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(width) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(0) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(height) + + frc::Rotation2d{ + units::radian_t{-std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + }; + viewplanes.clear(); + for (size_t i = 0; i < p.size(); i++) { + viewplanes.emplace_back(Eigen::Matrix{ + p[i].X().to(), p[i].Y().to(), p[i].Z().to()}); + } + } + + void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) { + avgErrorPx = newAvgErrorPx; + errorStdDevPx = newErrorStdDevPx; + } + + void SetFPS(units::hertz_t fps) { + frameSpeed = units::math::max(1 / fps, exposureTime); + } + + void SetExposureTime(units::second_t newExposureTime) { + exposureTime = newExposureTime; + frameSpeed = units::math::max(frameSpeed, exposureTime); + } + + void SetAvgLatency(units::second_t newAvgLatency) { + avgLatency = newAvgLatency; + } + + void SetLatencyStdDev(units::second_t newLatencyStdDev) { + latencyStdDev = newLatencyStdDev; + } + + int GetResWidth() const { return resWidth; } + + int GetResHeight() const { return resHeight; } + + int GetResArea() const { return resWidth * resHeight; } + + double GetAspectRatio() const { + return static_cast(resWidth) / static_cast(resHeight); + } + + Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } + + Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + + units::hertz_t GetFPS() const { return 1 / frameSpeed; } + + units::second_t GetFrameSpeed() const { return frameSpeed; } + + units::second_t GetExposureTime() const { return exposureTime; } + + units::second_t GetAverageLatency() const { return avgLatency; } + + units::second_t GetLatencyStdDev() const { return latencyStdDev; } + + double GetContourAreaPercent(const std::vector& points) { + return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) / + GetResArea() * 100; + } + + frc::Rotation2d GetPixelYaw(double pixelX) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - pixelX; + return frc::Rotation2d{fx, xOffset}; + } + + frc::Rotation2d GetPixelPitch(double pixelY) const { + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - pixelY; + return frc::Rotation2d{fy, -yOffset}; + } + + frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { + return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), + GetPixelYaw(point.x).Radians()}; + } + + frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - point.x; + + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - point.y; + + frc::Rotation2d yaw{fx, xOffset}; + frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset}; + return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; + } + + frc::Rotation2d GetHorizFOV() const { + frc::Rotation2d left = GetPixelYaw(0); + frc::Rotation2d right = GetPixelYaw(resWidth); + return left - right; + } + + frc::Rotation2d GetVertFOV() const { + frc::Rotation2d above = GetPixelPitch(0); + frc::Rotation2d below = GetPixelPitch(resHeight); + return below - above; + } + + frc::Rotation2d GetDiagFOV() const { + return frc::Rotation2d{ + units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + } + + std::pair, std::optional> GetVisibleLine( + const RotTrlTransform3d& camRt, const frc::Translation3d& a, + const frc::Translation3d& b) const { + frc::Translation3d relA = camRt.Apply(a); + frc::Translation3d relB = camRt.Apply(b); + + if (relA.X() <= 0_m && relB.X() <= 0_m) { + return {std::nullopt, std::nullopt}; + } + + Eigen::Matrix av{relA.X().to(), relA.Y().to(), + relA.Z().to()}; + Eigen::Matrix bv{relB.X().to(), relB.Y().to(), + relB.Z().to()}; + Eigen::Matrix abv = bv - av; + + bool aVisible = true; + bool bVisible = true; + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + double aVisibility = av.dot(normal); + if (aVisibility < 0) { + aVisible = false; + } + double bVisibility = bv.dot(normal); + if (bVisibility < 0) { + bVisible = false; + } + if (aVisibility <= 0 && bVisibility <= 0) { + return {std::nullopt, std::nullopt}; + } + } + + if (aVisible && bVisible) { + return {0, 1}; + } + + std::array intersections{std::nan(""), std::nan(""), + std::nan(""), std::nan("")}; + std::vector>> ipts{ + {std::nullopt, std::nullopt, std::nullopt, std::nullopt}}; + + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + Eigen::Matrix a_projn{}; + a_projn = (av.dot(normal) / normal.dot(normal)) * normal; + + if (std::abs(abv.dot(normal)) < 1e-5) { + continue; + } + intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn)); + + Eigen::Matrix apv{}; + apv = intersections[i] * abv; + Eigen::Matrix intersectpt{}; + intersectpt = av + apv; + ipts[i] = intersectpt; + + for (size_t j = 1; j < viewplanes.size(); j++) { + int oi = (i + j) % viewplanes.size(); + Eigen::Matrix onormal = viewplanes[oi]; + if (intersectpt.dot(onormal) < 0) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + + if (!ipts[i]) { + continue; + } + + for (int j = i - 1; j >= 0; j--) { + std::optional> oipt = ipts[j]; + if (!oipt) { + continue; + } + Eigen::Matrix diff{}; + diff = oipt.value() - intersectpt; + if (diff.cwiseAbs().maxCoeff() < 1e-4) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + } + + double inter1 = std::nan(""); + double inter2 = std::nan(""); + for (double inter : intersections) { + if (!std::isnan(inter)) { + if (std::isnan(inter1)) { + inter1 = inter; + } else { + inter2 = inter; + } + } + } + + if (!std::isnan(inter2)) { + double max = std::max(inter1, inter2); + double min = std::min(inter1, inter2); + if (aVisible) { + min = 0; + } + if (bVisible) { + max = 1; + } + return {min, max}; + } else if (!std::isnan(inter1)) { + if (aVisible) { + return {0, inter1}; + } + if (bVisible) { + return {inter1, 1}; + } + return {inter1, std::nullopt}; + } else { + return {std::nullopt, std::nullopt}; + } + } + + std::vector EstPixelNoise( + const std::vector& points) { + if (avgErrorPx == 0 && errorStdDevPx == 0) { + return points; + } + + std::vector noisyPts; + noisyPts.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + cv::Point2f p = points[i]; + float error = avgErrorPx + gaussian(generator) * errorStdDevPx; + float errorAngle = + uniform(generator) * 2 * std::numbers::pi - std::numbers::pi; + noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle), + p.y + error * std::sin(errorAngle)}); + } + return noisyPts; + } + + units::second_t EstLatency() { + return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, + 0_s); + } + + units::second_t EstSecUntilNextFrame() { + return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); + } + + static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; } + + static SimCameraProperties PI4_LIFECAM_320_240() { + SimCameraProperties prop{}; + prop.SetCalibration( + 320, 240, + (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906, + 0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.09957946553445934, -0.9166265114485799, 0.0019519890627236526, + -0.0036071725380870333, 1.5627234622420942}); + prop.SetCalibError(0.21, 0.0124); + prop.SetFPS(30_Hz); + prop.SetAvgLatency(30_ms); + prop.SetLatencyStdDev(10_ms); + return prop; + } + + static SimCameraProperties PI4_LIFECAM_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213, + 0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.12788470750464645, -1.2350335805796528, 0.0024990767286192732, + -0.0026958287600230705, 2.2951386729115537}); + prop.SetCalibError(0.26, 0.046); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(65_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096, + 0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.1917469998873756, -0.5142936883324216, + 0.012461562046896614, 0.0014084973492408186, + 0.35160648971214437}); + prop.SetCalibError(0.25, 0.05); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(35_ms); + prop.SetLatencyStdDev(8_ms); + return prop; + } + + static SimCameraProperties LL2_960_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 960, 720, + (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122, + 0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.189462064814501, -0.49903003669627627, + 0.007468423590519429, 0.002496885298683693, + 0.3443122090208624}); + prop.SetCalibError(0.35, 0.10); + prop.SetFPS(10_Hz); + prop.SetAvgLatency(50_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_1280_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 1280, 720, + (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737, + 0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.13730101577061535, -0.2904345656989261, + 8.32475714507539E-4, -3.694397782014239E-4, + 0.09487962227027584}); + prop.SetCalibError(0.37, 0.06); + prop.SetFPS(7_Hz); + prop.SetAvgLatency(60_ms); + prop.SetLatencyStdDev(20_ms); + return prop; + } + + private: + std::mt19937 generator{std::random_device{}()}; + std::normal_distribution gaussian{0.0, 1.0}; + std::uniform_real_distribution uniform{0.0, 1.0}; + + int resWidth; + int resHeight; + Eigen::Matrix camIntrinsics; + Eigen::Matrix distCoeffs; + double avgErrorPx; + double errorStdDevPx; + units::second_t frameSpeed{0}; + units::second_t exposureTime{0}; + units::second_t avgLatency{0}; + units::second_t latencyStdDev{0}; + std::vector> viewplanes{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h new file mode 100644 index 0000000000..bb2c453a88 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -0,0 +1,432 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "SimCameraProperties.h" +#include "photon/estimation/RotTrlTransform3d.h" + +namespace mathutil { +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} +} // namespace mathutil + +namespace photon { +namespace VideoSimUtil { +static constexpr int kNumTags16h5 = 30; + +static constexpr units::meter_t fieldLength{16.54175_m}; +static constexpr units::meter_t fieldWidth{8.0137_m}; + +static cv::Mat Get16h5TagImage(int id) { + wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id); + cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data}; + cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone(); + return markerClone; +} + +static std::unordered_map LoadAprilTagImages() { + std::unordered_map retVal{}; + for (int i = 0; i < kNumTags16h5; i++) { + cv::Mat tagImage = Get16h5TagImage(i); + retVal[i] = tagImage; + } + return retVal; +} + +static std::vector GetImageCorners(const cv::Size& size) { + std::vector retVal{}; + retVal.emplace_back(cv::Point2f{-0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f}); + retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f}); + return retVal; +} + +static std::vector Get16h5MarkerPts(int scale) { + cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}}; + roi16h5.x *= scale; + roi16h5.y *= scale; + roi16h5.width *= scale; + roi16h5.height *= scale; + std::vector pts = GetImageCorners(roi16h5.size()); + for (size_t i = 0; i < pts.size(); i++) { + cv::Point2f pt = pts[i]; + pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y}; + } + return pts; +} + +static std::vector Get16h5MarkerPts() { + return Get16h5MarkerPts(1); +} + +static const std::unordered_map kTag16h5Images = + LoadAprilTagImages(); +static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); + +[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, + const SimCameraProperties& prop) { + video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); + video.SetFPS(prop.GetFPS().to()); +} + +[[maybe_unused]] static void Warp165h5TagImage( + int tagId, const std::vector& dstPoints, bool antialiasing, + cv::Mat& destination) { + if (!kTag16h5Images.contains(tagId)) { + return; + } + cv::Mat tagImage = kTag16h5Images.at(tagId); + std::vector tagPoints{kTag16h5MarkPts}; + std::vector tagImageCorners{GetImageCorners(tagImage.size())}; + std::vector dstPointMat = dstPoints; + cv::Rect boundingRect = cv::boundingRect(dstPointMat); + cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat); + std::vector extremeCorners{}; + cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf); + boundingRect = cv::boundingRect(extremeCorners); + + double warpedContourArea = cv::contourArea(extremeCorners); + double warpedTagUpscale = + std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area()); + int warpStrat = cv::INTER_NEAREST; + + int supersampling = 6; + supersampling = static_cast(std::ceil(supersampling / warpedTagUpscale)); + supersampling = std::max(std::min(supersampling, 8), 1); + + cv::Mat scaledTagImage{}; + if (warpedTagUpscale > 2.0) { + warpStrat = cv::INTER_LINEAR; + int scaleFactor = static_cast(warpedTagUpscale / 3.0) + 2; + scaleFactor = std::max(std::min(scaleFactor, 40), 1); + scaleFactor *= supersampling; + cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor, + cv::INTER_NEAREST); + tagPoints = Get16h5MarkerPts(scaleFactor); + } else { + scaledTagImage = tagImage; + } + + boundingRect.x -= 1; + boundingRect.y -= 1; + boundingRect.width += 2; + boundingRect.height += 2; + if (boundingRect.x < 0) { + boundingRect.width += boundingRect.x; + boundingRect.x = 0; + } + if (boundingRect.y < 0) { + boundingRect.height += boundingRect.y; + boundingRect.y = 0; + } + boundingRect.width = + std::min(destination.size().width - boundingRect.x, boundingRect.width); + boundingRect.height = + std::min(destination.size().height - boundingRect.y, boundingRect.height); + if (boundingRect.width <= 0 || boundingRect.height <= 0) { + return; + } + + std::vector scaledDstPts{}; + if (supersampling > 1) { + cv::multiply(dstPointMat, + cv::Scalar{static_cast(supersampling), + static_cast(supersampling)}, + scaledDstPts); + boundingRect.x *= supersampling; + boundingRect.y *= supersampling; + boundingRect.width *= supersampling; + boundingRect.height *= supersampling; + } else { + scaledDstPts = dstPointMat; + } + + cv::subtract(scaledDstPts, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + scaledDstPts); + perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts); + + cv::Mat tempRoi{}; + cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(), + warpStrat); + + if (supersampling > 1) { + boundingRect.x /= supersampling; + boundingRect.y /= supersampling; + boundingRect.width /= supersampling; + boundingRect.height /= supersampling; + cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA); + } + + cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)}; + cv::subtract(extremeCorners, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + extremeCorners); + cv::Point2f tempCenter{}; + tempCenter.x = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.x / extremeCorners.size(); + }); + tempCenter.y = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.y / extremeCorners.size(); + }); + + for (auto& corner : extremeCorners) { + float xDiff = corner.x - tempCenter.x; + float yDiff = corner.y - tempCenter.y; + xDiff += 1 * mathutil::sgn(xDiff); + yDiff += 1 * mathutil::sgn(yDiff); + corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff}; + } + + std::vector extremeCornerInt{extremeCorners.begin(), + extremeCorners.end()}; + cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255}); + + cv::copyTo(tempRoi, destination(boundingRect), tempMask); +} + +static double GetScaledThickness(double thickness480p, + const cv::Mat& destinationMat) { + double scaleX = destinationMat.size().width / 640.0; + double scaleY = destinationMat.size().height / 480.0; + double minScale = std::min(scaleX, scaleY); + return std::max(thickness480p * minScale, 1.0); +} + +[[maybe_unused]] static void DrawInscribedEllipse( + const std::vector& dstPoints, const cv::Scalar& color, + cv::Mat& destination) { + cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints); + cv::ellipse(destination, rect, color, -1, cv::LINE_AA); +} + +static void DrawPoly(const std::vector& dstPoints, int thickness, + const cv::Scalar& color, bool isClosed, + cv::Mat& destination) { + std::vector intDstPoints{dstPoints.begin(), dstPoints.end()}; + std::vector> listOfListOfPoints; + listOfListOfPoints.emplace_back(intDstPoints); + if (thickness > 0) { + cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness, + cv::LINE_AA); + } else { + cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA); + } +} + +[[maybe_unused]] static void DrawTagDetection( + int id, const std::vector& dstPoints, cv::Mat& destination) { + double thickness = GetScaledThickness(1, destination); + DrawPoly(dstPoints, static_cast(thickness), cv::Scalar{0, 0, 255}, true, + destination); + cv::Rect2d rect{cv::boundingRect(dstPoints)}; + cv::Point2d textPt{rect.x + rect.width, rect.y}; + textPt.x += thickness; + textPt.y += thickness; + cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN, + 1.5 * thickness, cv::Scalar{0, 200, 0}, + static_cast(thickness), cv::LINE_AA); +} + +static std::vector> GetFieldWallLines() { + std::vector> list; + + const units::meter_t sideHt = 19.5_in; + const units::meter_t driveHt = 35_in; + const units::meter_t topHt = 78_in; + + // field floor + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, + frc::Translation3d{fieldLength, 0_m, 0_m}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, 0_m, 0_m}}); + + // right side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, 0_m}}); + + // red driverstation + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, topHt}, + frc::Translation3d{fieldLength, fieldWidth, topHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, driveHt}, + frc::Translation3d{fieldLength, fieldWidth, driveHt}}); + + // left side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}}); + + // blue driverstation + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{0_m, 0_m, topHt}, + frc::Translation3d{0_m, fieldWidth, topHt}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, driveHt}, + frc::Translation3d{0_m, fieldWidth, driveHt}}); + + return list; +} + +static std::vector> GetFieldFloorLines( + int subdivisions) { + std::vector> list; + const units::meter_t subLength = fieldLength / subdivisions; + const units::meter_t subWidth = fieldWidth / subdivisions; + + for (int i = 0; i < subdivisions; i++) { + list.emplace_back(std::vector{ + frc::Translation3d{0_m, subWidth * (i + 1), 0_m}, + frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); + list.emplace_back(std::vector{ + frc::Translation3d{subLength * (i + 1), 0_m, 0_m}, + frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); + } + return list; +} + +static std::vector> PolyFrom3dLines( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + const std::vector& trls, double resolution, + bool isClosed, cv::Mat& destination) { + resolution = std::hypot(destination.size().height, destination.size().width) * + resolution; + std::vector pts{trls}; + if (isClosed) { + pts.emplace_back(pts[0]); + } + std::vector> polyPointList{}; + + for (size_t i = 0; i < pts.size() - 1; i++) { + frc::Translation3d pta = pts[i]; + frc::Translation3d ptb = pts[i + 1]; + + std::pair, std::optional> inter = + prop.GetVisibleLine(camRt, pta, ptb); + if (!inter.second) { + continue; + } + + double inter1 = inter.first.value(); + double inter2 = inter.second.value(); + frc::Translation3d baseDelta = ptb - pta; + frc::Translation3d old_pta = pta; + if (inter1 > 0) { + pta = old_pta + baseDelta * inter1; + } + if (inter2 < 1) { + ptb = old_pta + baseDelta * inter2; + } + baseDelta = ptb - pta; + + std::vector poly = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb}); + cv::Point2d pxa = poly[0]; + cv::Point2d pxb = poly[1]; + + double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y); + int subdivisions = static_cast(pxDist / resolution); + frc::Translation3d subDelta = baseDelta / (subdivisions + 1); + std::vector subPts{}; + for (int j = 0; j < subdivisions; j++) { + subPts.emplace_back(pta + (subDelta * (j + 1))); + } + if (subPts.size() > 0) { + std::vector toAdd = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts); + poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end()); + } + + polyPointList.emplace_back(poly); + } + + return polyPointList; +} + +[[maybe_unused]] static void DrawFieldWireFrame( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + double resolution, double wallThickness, const cv::Scalar& wallColor, + int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor, + cv::Mat& destination) { + for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(floorThickness, destination))), + floorColor, false, destination); + } + } + for (const auto& trls : GetFieldWallLines()) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(wallThickness, destination))), + wallColor, false, destination); + } + } +} +} // namespace VideoSimUtil +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h new file mode 100644 index 0000000000..55988bc32f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -0,0 +1,282 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "photon/simulation/PhotonCameraSim.h" + +namespace photon { +class VisionSystemSim { + public: + explicit VisionSystemSim(std::string visionSystemName) { + std::string tableName = "VisionSystemSim-" + visionSystemName; + frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + } + std::optional GetCameraSim(std::string name) { + auto it = camSimMap.find(name); + if (it != camSimMap.end()) { + return std::make_optional(it->second); + } else { + return std::nullopt; + } + } + std::vector GetCameraSims() { + std::vector retVal; + for (auto const& cam : camSimMap) { + retVal.emplace_back(cam.second); + } + return retVal; + } + void AddCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + auto found = + camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (found == camSimMap.end()) { + camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] = + cameraSim; + camTrfMap.insert(std::make_pair( + std::move(cameraSim), + frc::TimeInterpolatableBuffer{bufferLength})); + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + } + } + void ClearCameras() { + camSimMap.clear(); + camTrfMap.clear(); + } + bool RemoveCamera(PhotonCameraSim* cameraSim) { + int numOfElementsRemoved = + camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (numOfElementsRemoved == 1) { + return true; + } else { + return false; + } + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { + return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, + units::second_t time) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + frc::TimeInterpolatableBuffer trfBuffer = + camTrfMap.at(cameraSim); + std::optional sample = trfBuffer.Sample(time); + if (!sample) { + return std::nullopt; + } else { + return std::make_optional( + frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})}); + } + } else { + return std::nullopt; + } + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim) { + return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim, + units::second_t time) { + auto robotToCamera = GetRobotToCamera(cameraSim, time); + if (!robotToCamera) { + return std::nullopt; + } else { + return std::make_optional(GetRobotPose(time) + robotToCamera.value()); + } + } + bool AdjustCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + return true; + } else { + return false; + } + } + void ResetCameraTransforms() { + for (const auto& pair : camTrfMap) { + ResetCameraTransforms(pair.first); + } + } + bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { + units::second_t now = frc::Timer::GetFPGATimestamp(); + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + auto trfBuffer = camTrfMap.at(cameraSim); + frc::Transform3d lastTrf{frc::Pose3d{}, + trfBuffer.Sample(now).value_or(frc::Pose3d{})}; + trfBuffer.Clear(); + AdjustCamera(cameraSim, lastTrf); + return true; + } else { + return false; + } + } + std::vector GetVisionTargets() { + std::vector all{}; + for (const auto& entry : targetSets) { + for (const auto& target : entry.second) { + all.emplace_back(target); + } + } + return all; + } + std::vector GetVisionTargets(std::string type) { + return targetSets[type]; + } + void AddVisionTargets(const std::vector& targets) { + AddVisionTargets("targets", targets); + } + void AddVisionTargets(std::string type, + const std::vector& targets) { + if (!targetSets.contains(type)) { + targetSets[type] = std::vector{}; + } + for (const auto& tgt : targets) { + targetSets[type].emplace_back(tgt); + } + } + void AddAprilTags(const frc::AprilTagFieldLayout& layout) { + std::vector targets; + for (const frc::AprilTag& tag : layout.GetTags()) { + targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), + photon::kAprilTag16h5, tag.ID}); + } + AddVisionTargets("apriltag", targets); + } + void ClearVisionTargets() { targetSets.clear(); } + void ClearAprilTags() { RemoveVisionTargets("apriltag"); } + void RemoveVisionTargets(std::string type) { targetSets.erase(type); } + std::vector RemoveVisionTargets( + const std::vector& targets) { + std::vector removedList; + for (auto& entry : targetSets) { + for (auto target : entry.second) { + auto it = std::find(targets.begin(), targets.end(), target); + if (it != targets.end()) { + removedList.emplace_back(target); + entry.second.erase(it); + } + } + } + return removedList; + } + frc::Pose3d GetRobotPose() { + return GetRobotPose(frc::Timer::GetFPGATimestamp()); + } + frc::Pose3d GetRobotPose(units::second_t timestamp) { + return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); + } + void ResetRobotPose(const frc::Pose2d& robotPose) { + ResetRobotPose(frc::Pose3d{robotPose}); + } + void ResetRobotPose(const frc::Pose3d& robotPose) { + robotPoseBuffer.Clear(); + robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); + } + frc::Field2d& GetDebugField() { return dbgField; } + void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + void Update(const frc::Pose3d& robotPose) { + for (auto& set : targetSets) { + std::vector posesToAdd{}; + for (auto& target : set.second) { + posesToAdd.emplace_back(target.GetPose().ToPose2d()); + } + dbgField.GetObject(set.first)->SetPoses(posesToAdd); + } + + units::second_t now = frc::Timer::GetFPGATimestamp(); + robotPoseBuffer.AddSample(now, robotPose); + dbgField.SetRobotPose(robotPose.ToPose2d()); + + std::vector allTargets{}; + for (const auto& set : targetSets) { + for (const auto& target : set.second) { + allTargets.emplace_back(target); + } + } + + std::vector visTgtPoses2d{}; + std::vector cameraPoses2d{}; + bool processed{false}; + for (const auto& entry : camSimMap) { + auto camSim = entry.second; + auto optTimestamp = camSim->ConsumeNextEntryTime(); + if (!optTimestamp) { + continue; + } else { + processed = true; + } + uint64_t timestampNt = optTimestamp.value(); + units::second_t latency = camSim->prop.EstLatency(); + units::second_t timestampCapture = + units::microsecond_t{static_cast(timestampNt)} - latency; + + frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture); + frc::Pose3d lateCameraPose = + lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); + cameraPoses2d.push_back(lateCameraPose.ToPose2d()); + + auto camResult = camSim->Process(latency, lateCameraPose, allTargets); + camSim->SubmitProcessedFrame(camResult, timestampNt); + for (const auto& target : camResult.GetTargets()) { + auto trf = target.GetBestCameraToTarget(); + if (trf == kEmptyTrf) { + continue; + } + visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d()); + } + } + if (processed) { + dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d); + } + if (cameraPoses2d.size() != 0) { + dbgField.GetObject("cameras")->SetPoses(cameraPoses2d); + } + } + + private: + std::unordered_map camSimMap{}; + static constexpr units::second_t bufferLength{1.5_s}; + std::unordered_map> + camTrfMap; + frc::TimeInterpolatableBuffer robotPoseBuffer{bufferLength}; + std::unordered_map> targetSets{}; + frc::Field2d dbgField{}; + const frc::Transform3d kEmptyTrf{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h new file mode 100644 index 0000000000..f8d4d6676b --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h @@ -0,0 +1,73 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include + +#include "photon/estimation/TargetModel.h" + +namespace photon { +class VisionTargetSim { + public: + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model) + : fiducialId(-1), pose(pose), model(model) {} + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id) + : fiducialId(id), pose(pose), model(model) {} + void SetPose(const frc::Pose3d& newPose) { pose = newPose; } + void SetModel(const TargetModel& newModel) { model = newModel; } + frc::Pose3d GetPose() const { return pose; } + TargetModel GetModel() const { return model; } + std::vector GetFieldVertices() const { + return model.GetFieldVertices(pose); + } + int fiducialId; + + bool operator<(const VisionTargetSim& right) const { + return pose.Translation().Norm() < right.pose.Translation().Norm(); + } + + bool operator==(const VisionTargetSim& other) const { + return units::math::abs(pose.Translation().X() - + other.GetPose().Translation().X()) < 1_in && + units::math::abs(pose.Translation().Y() - + other.GetPose().Translation().Y()) < 1_in && + units::math::abs(pose.Translation().Z() - + other.GetPose().Translation().Z()) < 1_in && + units::math::abs(pose.Rotation().X() - + other.GetPose().Rotation().X()) < 1_deg && + units::math::abs(pose.Rotation().Y() - + other.GetPose().Rotation().Y()) < 1_deg && + units::math::abs(pose.Rotation().Z() - + other.GetPose().Rotation().Z()) < 1_deg && + model.GetIsPlanar() == other.GetModel().GetIsPlanar(); + } + + private: + frc::Pose3d pose; + TargetModel model; +}; +} // namespace photon diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp new file mode 100644 index 0000000000..1c3f9eac58 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -0,0 +1,461 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "gtest/gtest.h" +#include "photon/PhotonUtils.h" +#include "photon/simulation/VisionSystemSim.h" + +class VisionSystemSimTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photon::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class VisionSystemSimTestWithParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface {}; +class VisionSystemSimTestDistanceParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + + // To the right, to the right + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + + frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, + frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + + frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(1.0); + cameraSim.SetMaxSightRange(10_m); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + + frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25); +} + +TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg)); + +TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { + units::foot_t distParam; + units::degree_t pitchParam; + units::foot_t heightParam; + std::tie(distParam, pitchParam, heightParam) = GetParam(); + + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; + frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; + frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, + frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + photon::VisionSystemSim visionSysSim{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AdjustCamera(&cameraSim, robotToCamera); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.Update(robotPose); + + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + photon::PhotonTrackedTarget target = res.GetBestTarget(); + + ASSERT_NEAR(0.0, target.GetYaw(), 0.5); + + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, VisionSystemSimTestDistanceParamsTest, + testing::Values(std::make_tuple(5_ft, -15.98_deg, 0_ft), + std::make_tuple(6_ft, -15.98_deg, 1_ft), + std::make_tuple(10_ft, -15.98_deg, 0_ft), + std::make_tuple(15_ft, -15.98_deg, 2_ft), + std::make_tuple(19.95_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -15.98_deg, 0_ft), + std::make_tuple(5_ft, -42_deg, 1_ft), + std::make_tuple(6_ft, -42_deg, 0_ft), + std::make_tuple(10_ft, -42_deg, 2_ft), + std::make_tuple(15_ft, -42_deg, 0.5_ft), + std::make_tuple(19.42_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -42_deg, 0_ft), + std::make_tuple(5_ft, -55_deg, 2_ft), + std::make_tuple(6_ft, -55_deg, 0_ft), + std::make_tuple(10_ft, -54_deg, 2.2_ft), + std::make_tuple(15_ft, -53_deg, 0_ft), + std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft))); + +TEST_F(VisionSystemSimTest, TestMultipleTargets) { + frc::Pose3d targetPoseL{ + frc::Translation3d{15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseC{ + frc::Translation3d{15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseR{ + frc::Translation3d{15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 2}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 3}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 4}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 5}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 6}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 7}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 8}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 9}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 10}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 11}}); + + frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + visionSysSim.Update(robotPose); + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + std::span tgtList = res.GetTargets(); + ASSERT_EQ(static_cast(11), tgtList.size()); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimation) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen; + cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen); + + Eigen::Matrix distEigen; + cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag16h5); + frc::Pose3d pose = frc::Pose3d{} + results.best; + ASSERT_NEAR(5, pose.X().to(), 0.01); + ASSERT_NEAR(1, pose.Y().to(), 0.01); + ASSERT_NEAR(0, pose.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); + frc::Pose3d pose2 = frc::Pose3d{} + results2.best; + ASSERT_NEAR(5, pose2.X().to(), 0.01); + ASSERT_NEAR(1, pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h new file mode 100644 index 0000000000..a0a0fb0e61 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +namespace photon { +class CameraTargetRelation { + public: + const frc::Pose3d camPose; + const frc::Transform3d camToTarg; + const units::meter_t camToTargDist; + const units::meter_t camToTargDistXY; + const frc::Rotation2d camToTargYaw; + const frc::Rotation2d camToTargPitch; + + const frc::Rotation2d camToTargAngle; + + const frc::Transform3d targToCam; + const frc::Rotation2d targToCamYaw; + const frc::Rotation2d targToCamPitch; + + const frc::Rotation2d targToCamAngle; + + CameraTargetRelation(const frc::Pose3d& cameraPose, + const frc::Pose3d& targetPose) + : camPose(cameraPose), + camToTarg(frc::Transform3d{cameraPose, targetPose}), + camToTargDist(camToTarg.Translation().Norm()), + camToTargDistXY(units::math::hypot(camToTarg.Translation().X(), + camToTarg.Translation().Y())), + camToTargYaw(frc::Rotation2d{camToTarg.X().to(), + camToTarg.Y().to()}), + camToTargPitch(frc::Rotation2d{camToTargDistXY.to(), + -camToTarg.Z().to()}), + camToTargAngle(frc::Rotation2d{units::math::hypot( + camToTargYaw.Radians(), camToTargPitch.Radians())}), + targToCam(frc::Transform3d{targetPose, cameraPose}), + targToCamYaw(frc::Rotation2d{targToCam.X().to(), + targToCam.Y().to()}), + targToCamPitch(frc::Rotation2d{camToTargDistXY.to(), + -targToCam.Z().to()}), + targToCamAngle(frc::Rotation2d{units::math::hypot( + targToCamYaw.Radians(), targToCamPitch.Radians())}) {} +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h new file mode 100644 index 0000000000..2d79e661e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -0,0 +1,291 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "RotTrlTransform3d.h" + +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { +namespace OpenCVHelp { + +static frc::Rotation3d NWU_TO_EDN{ + (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()}; +static frc::Rotation3d EDN_TO_NWU{ + (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; + +static std::vector GetConvexHull( + const std::vector& points) { + std::vector outputHull{}; + cv::convexHull(points, outputHull); + std::vector convexPoints; + for (size_t i = 0; i < outputHull.size(); i++) { + convexPoints.push_back(points[outputHull[i]]); + } + return convexPoints; +} + +static cv::RotatedRect GetMinAreaRect(const std::vector& points) { + return cv::minAreaRect(points); +} + +static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) { + return trl.RotateBy(NWU_TO_EDN); +} + +static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) { + return -NWU_TO_EDN + (rot + NWU_TO_EDN); +} + +static std::vector TranslationToTVec( + const std::vector& translations) { + std::vector retVal; + retVal.reserve(translations.size()); + for (size_t i = 0; i < translations.size(); i++) { + frc::Translation3d trl = TranslationNWUtoEDN(translations[i]); + retVal.emplace_back(cv::Point3f{trl.X().to(), trl.Y().to(), + trl.Z().to()}); + } + return retVal; +} + +static std::vector RotationToRVec( + const frc::Rotation3d& rotation) { + std::vector retVal{}; + frc::Rotation3d rot = RotationNWUtoEDN(rotation); + retVal.emplace_back(cv::Point3d{ + rot.GetQuaternion().ToRotationVector()(0), + rot.GetQuaternion().ToRotationVector()(1), + rot.GetQuaternion().ToRotationVector()(2), + }); + return retVal; +} + +[[maybe_unused]] static cv::Point2f AvgPoint(std::vector points) { + if (points.size() == 0) { + return cv::Point2f{}; + } + cv::reduce(points, points, 0, cv::REDUCE_AVG); + return points[0]; +} + +[[maybe_unused]] static std::vector> PointsToCorners( + const std::vector& points) { + std::vector> retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(std::make_pair(points[i].x, points[i].y)); + } + return retVal; +} + +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector>& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second}); + } + return retVal; +} + +[[maybe_unused]] static cv::Rect GetBoundingRect( + const std::vector& points) { + return cv::boundingRect(points); +} + +static std::vector ProjectPoints( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const RotTrlTransform3d& camRt, + const std::vector& objectTranslations) { + std::vector objectPoints = TranslationToTVec(objectTranslations); + std::vector rvec = RotationToRVec(camRt.GetRotation()); + std::vector tvec = TranslationToTVec({camRt.GetTranslation()}); + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + std::vector imagePoints{}; + cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat, + imagePoints); + return imagePoints; +} + +template +static std::vector ReorderCircular(const std::vector elements, + bool backwards, int shiftStart) { + size_t size = elements.size(); + int dir = backwards ? -1 : 1; + std::vector reordered{elements}; + for (size_t i = 0; i < size; i++) { + int index = (i * dir + shiftStart * dir) % size; + if (index < 0) { + index = size + index; + } + reordered[i] = elements[index]; + } + return reordered; +} + +static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) { + return trl.RotateBy(EDN_TO_NWU); +} + +static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) { + return -EDN_TO_NWU + (rot + EDN_TO_NWU); +} + +static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; + tvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]}, + units::meter_t{data[1]}, + units::meter_t{data[2]}}); +} + +static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; + rvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]}, + units::radian_t{data[1]}, + units::radian_t{data[2]}}); +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_Square( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + modelTrls = ReorderCircular(modelTrls, true, -1); + imagePoints = ReorderCircular(imagePoints, true, -1); + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs; + std::vector tvecs; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + cv::Vec2d errors{}; + frc::Transform3d best{}; + std::optional alt{std::nullopt}; + + for (int tries = 0; tries < 2; tries++) { + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec, + reprojectionError); + + errors = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (tvecs.size() > 1) { + alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)), + RVecToRotation(rvecs[1])}; + } + + if (!std::isnan(errors[0])) { + break; + } else { + cv::Point2f pt = imagePoints[0]; + pt.x -= 0.001f; + pt.y -= 0.001f; + imagePoints[0] = pt; + } + } + + if (std::isnan(errors[0])) { + fmt::print("SolvePNP_Square failed!\n"); + } + if (alt) { + photon::PNPResult result; + result.best = best; + result.alt = alt.value(); + result.ambiguity = errors[0] / errors[1]; + result.bestReprojErr = errors[0]; + result.altReprojErr = errors[1]; + result.isPresent = true; + return result; + } else { + photon::PNPResult result; + result.best = best; + result.bestReprojErr = errors[0]; + result.isPresent = true; + return result; + } +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs{}; + std::vector tvecs{}; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + float error = 0; + frc::Transform3d best{}; + + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec, + reprojectionError); + + error = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (std::isnan(error)) { + fmt::print("SolvePNP_Square failed!\n"); + } + photon::PNPResult result; + result.best = best; + result.bestReprojErr = error; + result.isPresent = true; + return result; +} +} // namespace OpenCVHelp +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h new file mode 100644 index 0000000000..99e19bf2c3 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include + +namespace photon { +class RotTrlTransform3d { + public: + RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl) + : trl(trl), rot(rot) {} + RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) + : trl(last.Translation() - initial.Translation().RotateBy(rot)), + rot(last.Rotation() - initial.Rotation()) {} + explicit RotTrlTransform3d(const frc::Transform3d& trf) + : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} + RotTrlTransform3d() + : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {} + + static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) { + return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse(); + } + + RotTrlTransform3d Inverse() const { + frc::Rotation3d invRot = -rot; + frc::Translation3d invTrl = -(trl.RotateBy(invRot)); + return RotTrlTransform3d{invRot, invTrl}; + } + + frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; } + + frc::Translation3d GetTranslation() const { return trl; } + + frc::Rotation3d GetRotation() const { return rot; } + + frc::Translation3d Apply(const frc::Translation3d& trlToApply) const { + return trlToApply.RotateBy(rot) + trl; + } + + std::vector ApplyTrls( + const std::vector& trls) const { + std::vector retVal; + retVal.reserve(trls.size()); + for (const auto& currentTrl : trls) { + retVal.emplace_back(Apply(currentTrl)); + } + return retVal; + } + + frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const { + return rotToApply + rot; + } + + std::vector ApplyTrls( + const std::vector& rots) const { + std::vector retVal; + retVal.reserve(rots.size()); + for (const auto& currentRot : rots) { + retVal.emplace_back(Apply(currentRot)); + } + return retVal; + } + + frc::Pose3d Apply(const frc::Pose3d& poseToApply) const { + return frc::Pose3d{Apply(poseToApply.Translation()), + Apply(poseToApply.Rotation())}; + } + + std::vector ApplyPoses( + const std::vector& poses) const { + std::vector retVal; + retVal.reserve(poses.size()); + for (const auto& currentPose : poses) { + retVal.emplace_back(Apply(currentPose)); + } + return retVal; + } + + private: + const frc::Translation3d trl; + const frc::Rotation3d rot; +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h new file mode 100644 index 0000000000..64392fdb11 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h @@ -0,0 +1,115 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include + +#include "RotTrlTransform3d.h" + +namespace photon { +class TargetModel { + public: + TargetModel(units::meter_t width, units::meter_t height) + : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, height / 2.0}, + frc::Translation3d{0_m, -width / 2.0, height / 2.0}}), + isPlanar(true), + isSpherical(false) {} + + TargetModel(units::meter_t length, units::meter_t width, + units::meter_t height) + : TargetModel({ + frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0}, + }) {} + + explicit TargetModel(units::meter_t diameter) + : vertices({ + frc::Translation3d{0_m, -diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, -diameter / 2.0}, + frc::Translation3d{0_m, diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, diameter / 2.0}, + }), + isPlanar(false), + isSpherical(true) {} + + explicit TargetModel(const std::vector& verts) + : isSpherical(false) { + if (verts.size() <= 2) { + vertices = std::vector(); + isPlanar = false; + } else { + bool cornersPlanar = true; + for (const auto& corner : verts) { + if (corner.X() != 0_m) { + cornersPlanar = false; + } + } + isPlanar = cornersPlanar; + } + vertices = verts; + } + + std::vector GetFieldVertices( + const frc::Pose3d& targetPose) const { + RotTrlTransform3d basisChange{targetPose.Rotation(), + targetPose.Translation()}; + std::vector retVal; + retVal.reserve(vertices.size()); + for (const auto& vert : vertices) { + retVal.emplace_back(basisChange.Apply(vert)); + } + return retVal; + } + + static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl, + const frc::Translation3d& cameraTrl) { + frc::Translation3d relCam = cameraTrl - tgtTrl; + frc::Rotation3d orientToCam = frc::Rotation3d{ + 0_rad, + frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to(), + -relCam.Z().to()} + .Radians(), + frc::Rotation2d{relCam.X().to(), relCam.Y().to()} + .Radians()}; + return frc::Pose3d{tgtTrl, orientToCam}; + } + + std::vector GetVertices() const { return vertices; } + bool GetIsPlanar() const { return isPlanar; } + bool GetIsSpherical() const { return isSpherical; } + + private: + std::vector vertices; + bool isPlanar; + bool isSpherical; +}; + +static const TargetModel kAprilTag16h5{6_in, 6_in}; +static const TargetModel kAprilTag36h11{6.5_in, 6.5_in}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h new file mode 100644 index 0000000000..cb3ae3bdc5 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -0,0 +1,121 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "OpenCVHelp.h" +#include "TargetModel.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { +namespace VisionEstimation { + +static std::vector GetVisibleLayoutTags( + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout) { + std::vector retVal{}; + for (const auto& tag : visTags) { + int id = tag.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + } + } + return retVal; +} + +static PNPResult EstimateCamPosePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + if (visTags.size() == 0) { + return PNPResult(); + } + + std::vector> corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return PNPResult{}; + } + + std::vector points = OpenCVHelp::CornersToPoints(corners); + + if (knownTags.size() == 1) { + PNPResult camToTag = OpenCVHelp::SolvePNP_Square( + cameraMatrix, distCoeffs, tagModel.GetVertices(), points); + if (!camToTag.isPresent) { + return PNPResult{}; + } + frc::Pose3d bestPose = + knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + frc::Pose3d altPose{}; + if (camToTag.ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + } + frc::Pose3d o{}; + PNPResult result{}; + result.best = frc::Transform3d{o, bestPose}; + result.alt = frc::Transform3d{o, altPose}; + result.ambiguity = camToTag.ambiguity; + result.bestReprojErr = camToTag.bestReprojErr; + result.altReprojErr = camToTag.altReprojErr; + return result; + } else { + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, + objectTrls, points); + if (!camToOrigin.isPresent) { + return PNPResult{}; + } else { + PNPResult result{}; + result.best = camToOrigin.best.Inverse(), + result.alt = camToOrigin.alt.Inverse(), + result.ambiguity = camToOrigin.ambiguity; + result.bestReprojErr = camToOrigin.bestReprojErr; + result.altReprojErr = camToOrigin.altReprojErr; + result.isPresent = true; + return result; + } + } +} + +} // namespace VisionEstimation +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h new file mode 100644 index 0000000000..ff647cd7cf --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace photon { +class NTTopicSet { + public: + std::shared_ptr subTable; + nt::RawPublisher rawBytesEntry; + + nt::IntegerPublisher pipelineIndexPublisher; + nt::IntegerSubscriber pipelineIndexRequestSub; + + nt::BooleanTopic driverModeEntry; + nt::BooleanPublisher driverModePublisher; + nt::BooleanSubscriber driverModeSubscriber; + + nt::DoublePublisher latencyMillisEntry; + nt::BooleanPublisher hasTargetEntry; + nt::DoublePublisher targetPitchEntry; + nt::DoublePublisher targetYawEntry; + nt::DoublePublisher targetAreaEntry; + nt::DoubleArrayPublisher targetPoseEntry; + nt::DoublePublisher targetSkewEntry; + + nt::DoublePublisher bestTargetPosX; + nt::DoublePublisher bestTargetPosY; + + nt::IntegerTopic heartbeatTopic; + nt::IntegerPublisher heartbeatPublisher; + + nt::DoubleArrayPublisher cameraIntrinsicsPublisher; + nt::DoubleArrayPublisher cameraDistortionPublisher; + + void UpdateEntries() { + nt::PubSubOptions options; + options.periodic = 0.01; + options.sendAll = true; + rawBytesEntry = + subTable->GetRawTopic("rawBytes").Publish("rawBytes", options); + + pipelineIndexPublisher = + subTable->GetIntegerTopic("pipelineIndexState").Publish(); + pipelineIndexRequestSub = + subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0); + + driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish(); + driverModeSubscriber = + subTable->GetBooleanTopic("driverModeRequest").Subscribe(0); + + driverModeSubscriber.GetTopic().Publish().SetDefault(false); + + latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish(); + hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish(); + + targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish(); + targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish(); + targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish(); + targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish(); + targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish(); + + bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish(); + bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish(); + + heartbeatTopic = subTable->GetIntegerTopic("heartbeat"); + heartbeatPublisher = heartbeatTopic.Publish(); + + cameraIntrinsicsPublisher = + subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish(); + cameraDistortionPublisher = + subTable->GetDoubleArrayTopic("cameraDistortion").Publish(); + } + + private: +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index e97f6c8d71..49f73ed53b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -27,15 +27,15 @@ class PNPResult { public: // This could be wrapped in an std::optional, but chose to do it this way to // mirror Java - bool isPresent; + bool isPresent{false}; - frc::Transform3d best; - double bestReprojErr; + frc::Transform3d best{}; + double bestReprojErr{0}; - frc::Transform3d alt; - double altReprojErr; + frc::Transform3d alt{}; + double altReprojErr{0}; - double ambiguity; + double ambiguity{0}; bool operator==(const PNPResult& other) const; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore new file mode 100644 index 0000000000..34878ab18c --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..a6e62683de --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md new file mode 100644 index 0000000000..3d5a824cad --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle new file mode 100644 index 0000000000..8eeef20126 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -0,0 +1,121 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" + + id "com.dorongold.task-tree" version "2.1.0" +} + +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' +wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' + +repositories { + mavenLocal() + jcenter() +} + +apply from: "${rootDir}/../shared/examples_common.gradle" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamOrDefault(5940) + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcCpp + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Set to true to run simulation in debug mode +wpi.cpp.debugSimulation = false + +// Default enable simgui +wpi.sim.addGui().defaultEnabled = true +// Enable DS but not by default +wpi.sim.addDriverstation() + +model { + components { + frcUserProgram(NativeExecutableSpec) { + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + + // Set deploy task to deploy this component + deployArtifact.component = it + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + // Enable simulation for this component + wpi.sim.enable(it) + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew @@ -0,0 +1,241 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle new file mode 100644 index 0000000000..44fbca7512 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json new file mode 100644 index 0000000000..c4b7efd3d8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json new file mode 100644 index 0000000000..1605887709 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json @@ -0,0 +1,64 @@ +{ + "Docking": { + "Data": [] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "910", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "1550", + "xpos": "-1602", + "ypos": "79" + } + }, + "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "199,200", + "Size": "1342,628" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "5,540", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "359,95", + "Size": "796,240" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "865,52", + "Size": "830,620" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..e1ba9acce8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json @@ -0,0 +1,57 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "width": 0.30000001192092896 + }, + "apriltag": { + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "bottom": 544, + "builtin": "2023 Charged Up", + "cameras": { + "arrowSize": 19, + "arrowWeight": 1.0, + "style": "Hidden" + }, + "height": 8.013679504394531, + "left": 46, + "right": 1088, + "top": 36, + "visibleTargetPoses": { + "image": "tag-blue.png" + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drive": { + "open": true + }, + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp new file mode 100644 index 0000000000..4505740955 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp @@ -0,0 +1,123 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "Robot.h" + +#include + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + drivetrain.Periodic(); + + auto visionEst = vision.GetEstimatedGlobalPose(); + if (visionEst.has_value()) { + auto est = visionEst.value(); + auto estPose = est.estimatedPose.ToPose2d(); + auto estStdDevs = vision.GetEstimationStdDevs(estPose); + drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp, + estStdDevs); + } + + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() { + autoTimer.Restart(); + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + +void Robot::AutonomousPeriodic() { + if (autoTimer.Get() < 10_s) { + drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); + } else { + autoTimer.Stop(); + drivetrain.Stop(); + } +} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() {} + +void Robot::TeleopPeriodic() { + double forward = -controller.GetLeftY() * kDriveSpeed; + if (std::abs(forward) < 0.1) { + forward = 0; + } + forward = forwardLimiter.Calculate(forward); + + double strafe = -controller.GetLeftX() * kDriveSpeed; + if (std::abs(strafe) < 0.1) { + strafe = 0; + } + strafe = strafeLimiter.Calculate(strafe); + + double turn = -controller.GetRightX() * kDriveSpeed; + if (std::abs(turn) < 0.1) { + turn = 0; + } + turn = turnLimiter.Calculate(turn); + + drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, + strafe * constants::Swerve::kMaxLinearSpeed, + turn * constants::Swerve::kMaxAngularSpeed, true); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..bf33d7189d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,211 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDrive.h" + +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, openLoop, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); } + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp) { + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); +} + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); +} + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..781c28a338 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,146 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveModule.h" + +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h new file mode 100644 index 0000000000..70be37d406 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +static constexpr units::meter_t kTrackWidth{18.5_in}; +static constexpr units::meter_t kTrackLength{18.5_in}; +static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +static constexpr units::meter_t kWheelDiameter{4_in}; +static constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +static constexpr double kDriveGearRatio = 6.75; +static constexpr double kSteerGearRatio = 12.8; + +static constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +static constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +static constexpr double kDriveKP = 1.0; +static constexpr double kDriveKI = 0.0; +static constexpr double kDriveKD = 0.0; + +static constexpr double kSteerKP = 20.0; +static constexpr double kSteerKI = 0.0; +static constexpr double kSteerKD = 0.25; + +static const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +static const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +static const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +static const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h new file mode 100644 index 0000000000..7a3697494a --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h @@ -0,0 +1,62 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#include "Vision.h" +#include "subsystems/SwerveDrive.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; + + private: + SwerveDrive drivetrain{}; + Vision vision{}; + frc::XboxController controller{0}; + frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; + frc::Timer autoTimer{}; + double kDriveSpeed{0.6}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h new file mode 100644 index 0000000000..619b34ade7 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h @@ -0,0 +1,152 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class Vision { + public: + Vision() { + photonEstimator.SetMultiTagFallbackStrategy( + photon::PoseStrategy::LOWEST_AMBIGUITY); + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(15_Hz); + cameraProp->SetAvgLatency(50_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = std::make_shared(camera.get(), + *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), robotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { + return camera->GetLatestResult(); + } + + std::optional GetEstimatedGlobalPose() { + auto visionEst = photonEstimator.Update(); + units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp(); + bool newResult = + units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s; + if (frc::RobotBase::IsSimulation()) { + if (visionEst.has_value()) { + GetSimDebugField() + .GetObject("VisionEstimation") + ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + } else { + if (newResult) { + GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); + } + } + } + if (newResult) { + lastEstTimestamp = latestTimestamp; + } + return visionEst; + } + + Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix estStdDevs = + constants::Vision::kSingleTagStdDevs; + auto targets = GetLatestResult().GetTargets(); + int numTags = 0; + units::meter_t avgDist = 0_m; + for (const auto& tgt : targets) { + auto tagPose = + photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); + if (tagPose.has_value()) { + numTags++; + avgDist += tagPose.value().ToPose2d().Translation().Distance( + estimatedPose.Translation()); + } + } + if (numTags == 0) { + return estStdDevs; + } + avgDist /= numTags; + if (numTags > 1) { + estStdDevs = constants::Vision::kMultiTagStdDevs; + } + if (numTags == 1 && avgDist > 4_m) { + estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()) + .finished(); + } else { + estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30)); + } + return estStdDevs; + } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m}, + frc::Rotation3d{}}; + photon::PhotonPoseEstimator photonEstimator{ + LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + photon::PhotonCamera{"photonvision"}, robotToCam}; + std::shared_ptr camera{photonEstimator.GetCamera()}; + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + units::second_t lastEstTimestamp{0_s}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..1e3d26b32f --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp new file mode 100644 index 0000000000..031d1ce96b --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp @@ -0,0 +1,34 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000000000000000000000000000000000..04b9e4f7b7241c94a87249c24fb09c2577b2cf44 GIT binary patch literal 4711 zcmeHL`BPI@6uy8KL`Weqf;O_r;;<=N1zgHf6rvI=C?--RfQSo_M3w-8rcOr_vEWE? z83-NQ0%HxENFvITK^Bq4b{w#p7?~hg1VIc1WRaemK>H80{V6|W&U^2E_nhzC`_6aX zjbIPW-`H@aAwtNQ8sHaxE-pV>SB!(F;Vy4iC{BwiC+C(?cnn+J4 z=~|}*Iqum`u{c(=U9f^de>ycFnNIy>ugiMoaC09SnGruQoMENu zA`=@WLTCLb$omps4;c`Fr;iN*Sp_uephW@?#IrKhM<%vfkah{QZ-KTDv|8~0%c0wZ zmm)UYQXJGh7@ww_-{02TJC;?Qdgiqx<3`cZ>9Kcz$}G=V#>(qYO{c0RC*Ms?d+CeL zQ7I^KhR6<+J+L0(_1!!-dyXoaC<%~x%bK5$jZ*Cs1|x|2$k?*z^;f5I zst3L;gceZl9V%*bP$K4y@uHKF@!vpuaL&ILW)HbLRhT6_SrQE`BD8sL{0bNmT_T_O zfv?9SlM)WXV|KVJ?j2s&A0_cUP>)%t$QYh_JiL&k05JReoARyDR(d2N`fGo{J+|-f z{}QZ&EE9sCAaRg%T}ZXe6l^BFLU9h7Qm1eG!K>IFU)x_qa`geUS%f2(0dDV_J9W2V zY?Gi`h39q+HOUhj)&SA3g*WAGIFh}H9=-$SxJNf_#Xb9I0-u{j*^pxO?YB-)<$| zOld<$22kL+*%0W0>gIgDUNhf=8Qq^8-YJCc9dk{V;hE#Xyhx>#M<&pL0rMhX@9r^R zi!UYzmQVD_1$rpuMwtBS27zAlwMHFEVhQ<{+7It1*V@P`+G2Ejb^2n>|zmaC#*!d)S zjuT4gB_TTK$ndWhx@-yNQAB+<>Hb2*yxYL$KmAI%HqB*uy$l@Dp{e>tIcfUR+}PeY zO<6gqHv#Rhv}A3uA(*dLqS8nbn>#%!?xj(X)F`f)6|`7>g-Ti1Y)j~|A(YTQg*{WJ z|M)w}D$VSegxI*LWq7ILUvVcCS#Fv|H`(c%YPS-dzBl$}%tRB@T`(VA`sm<^d4Du^$0=5e z2F%N??ppv$cecAMM0Z+uzG@&u&7`9{f9|)4_StOrGK;#?D&hSft7$LC)o)Q=*#!5r z%Ps<|y>M6Zpj2@DzXlg8oV=sG?shBf2ud!APp=-`5jBUh;o zi$u*tWgM=&%>Cg)3-~1EH63`Q8FKEt+^ZS7J(i-@1)HJ3mqj_zQ10H(S3<(L9*>o1 zeCKa$k!3-}t{*RAJ!wtDVaL1cHgEI8Rc)szum{%+BC7=tgksDRJe|2*L<_>y@<^;= zEF&FsVU}7>7{WRzDO9?nMwpGQZ;cR3k?V2R)FvUDfF;P_){{2SOesSk88iX=j4v5f z%hy|=NpcN@$PYv`7S7Ir#FBul@^bi+R1B@enULbCFyzk=cIJTR0$WA@F1VkINZNb` z+)uW?xr6ml=>n=MeI$^mMYzg6BagNQqLi->EpD>{rM7F7-G)r)I-}yPFl~7(CRn}< zUp{=3_yN@G^BK==*+kYL$9>!UyWJwMsaMJB_CSA0Pn(>2n?2CGRy-k58 zhlpkmzZW0+cmV4{$5pCa`C=}mG9&%>nvcuk2m3M!ph+q_C6g?Ig4G@(v`e5pN@@?v k+Jo}Hd{91lsCH^;si4Yp)^TGd{5gcE+i8BKzRa_K0jU-?_5c6? literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000000000000000000000000000000000..63029fcf205d2a1b52eb450aa1e26fbdfd310586 GIT binary patch literal 4698 zcmeHLYg3a~5I*5jKx+ge)e0&gii(5a6%bGmxikcdU<|@&07sk_2r+;_t4$FlTolv+ zLJ^(TkqjkV6#{Ze#3)q>+DXCEM4%CpMhuogNfFWR2{8QwZ9nBhX5Vx6d3T@Pb9T>p zlk<77|Dpve79fNc1qS%;N63JQkm1LsAHj@=-}q$^1lE3kA5_z0t%iY7oHxZAq56V_ z@1l)iY?c%d!9s}GiGKvk^`Eg3vUnBf>m8nUSR?biL^`;P9jzprnN`{yT`j)3bji&n zq+iLl$5s<7?d+31ioFEuqqoH*`|qAf-5pZ&g1Y~Lf+`R3G$R_q-!ONX&=hZlhZum`}dj;CRSR;k<)Syudg#^ zMg|S3ckPMD{j_OwwMHcwjV7fD%et%czNsd}1}T(}>=Q3GecN=tQT_aSkg^X$R}6$a zzCO8*B{d_jRUrDed|F9}m>}}eP)5XZ12`6Ft}cQgI47tvo0S!$hj29T^DE_BIH0k0 z?DK*%!*QMRHcSt7MBy`|V{6f%C;HG8Z{OLLcVn3zc7j zv3Vqx$^m3me_o3^D-?(}yN9)?NZ^S=tM|g55+CNKp&jj7fBlSOuC*eV2GAzy$5qjT+iMYgD&@99f?WkOReAidxIABHq-y7ttAA-E!fn zbglYLiJC;NJAXbQEoimdb%}Q5xpMsbzsfC`qU9DQn*v;vzShouV#31k?~b;TOpx=2 zXC+&64DWMcYNyt+ieq@6<4RwvT*0yab6tK4qIGzJ?ZZp&Ur>Sb4YexS98@^ao|;zMBbpWans#s)jyrR{7tah0eJ zE|%^>HAk%&WnI}JwZb-YU93slr^lNh$vBZ4cMwhrRx@^u!R@L0N$T54nkbQ0vS=A4lJO@&t4C_q4k_yyE z`*B4!o$FQG;z~XArOviZoCL28V&}K3WyuYPLB(xv)FlE6JV(%ltF%}wV`4=S#&qJ> zxyDseDNq|yie+ltSGH1l&#_FQ@*eB@iUmW3%~qK+RL~&VdB2r)P#mtDff$@RU%b}! zBC0~;j^Ej#&}4x8Gal2-BIDNLTqV^-_dqPu;4WydNLYm-_Bu!;Zit~&-UQsb+5S0# zDLfA02dkh3qrr&sRA9EJ%@|&!xsCS$h<*cS&}=O*r`~^&{3)2?XQf4c0)5BPW}q0h zjwn+@8LS```@o|?M7h&7jv@QpZk{Ssk>CkvqxduNR>!4X*EoVVko70yzYA?02Kq)5 zGYqaMC#tg5D_Ij`WrLGvTR*%#Hk~F+pV=Vqd$epN^cE#qd}kw#XQ2OB(0?z%_SAne l>QkWK1^;Rx47a6?FD6iOngjQS4#WRJC~#k}Z;cN<^KVFxHsJsO literal 0 HcmV?d00001 diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 66106c6057..dd8a5bec32 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -88,6 +88,8 @@ model { } } + nativeUtils.useRequiredLibrary(it, "cscore_shared") + nativeUtils.useRequiredLibrary(it, "cameraserver_shared") nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") From 6b8882fe53c66ba77ed29cc1a9a81d61a45c0353 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 15:26:00 -0500 Subject: [PATCH 16/20] Report cpp usage (#1045) --- .../src/main/native/cpp/photon/PhotonCamera.cpp | 7 ++++++- .../main/native/cpp/photon/PhotonPoseEstimator.cpp | 14 ++++++++++++-- .../src/main/native/include/photon/PhotonCamera.h | 1 + .../native/include/photon/PhotonPoseEstimator.h | 2 ++ 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index ff26f74fb8..02d9b46527 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -24,6 +24,8 @@ #include "photon/PhotonCamera.h" +#include + #include #include #include @@ -69,7 +71,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, rootTable->GetBooleanTopic("driverModeRequest").Publish()), m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}), path(rootTable->GetPath()), - m_cameraName(cameraName) {} + m_cameraName(cameraName) { + HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; +} PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f2ff30f7ea..30bc6de2df 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -24,6 +24,8 @@ #include "photon/PhotonPoseEstimator.h" +#include + #include #include #include @@ -68,7 +70,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, PoseStrategy strat, PhotonCamera&& cam, @@ -79,7 +85,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 582458e014..99aa52c61b 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -205,6 +205,7 @@ class PhotonCamera { private: units::second_t lastVersionCheckTime = 0_s; inline static bool VERSION_CHECK_ENABLED = true; + inline static int InstanceCount = 0; void VerifyVersion(); }; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index cdb60f6cee..2c863330bb 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -231,6 +231,8 @@ class PhotonPoseEstimator { units::second_t poseCacheTimestamp; + inline static int InstanceCount = 0; + inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } std::optional Update( From 82b82fe2f6f09a834de73414a0ab246aafd9e728 Mon Sep 17 00:00:00 2001 From: Programmers3539 Date: Sat, 16 Dec 2023 13:11:55 -0800 Subject: [PATCH 17/20] Raspberry Pi 5 Support (#1008) Deals with new otherpaths on pi 5 CSI cameras and bumps libcamera driver to latest from the pi 5 PR --------- Co-authored-by: Matt --- .../org/photonvision/raspi/LibCameraJNI.java | 3 + .../vision/camera/LibcameraGpuSettables.java | 3 + .../vision/processes/VisionSourceManager.java | 75 +++++++++++++- .../processes/VisionSourceManagerTest.java | 95 ++++++++++++++++-- .../nativelibraries/libphotonlibcamera.so | Bin 141664 -> 167016 bytes 5 files changed, 163 insertions(+), 13 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java index a75ae2e579..8a12aeabfa 100644 --- a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java +++ b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java @@ -64,6 +64,7 @@ public enum SensorModel { Disconnected, OV5647, // Picam v1 IMX219, // Picam v2 + IMX708, // Picam v3 IMX477, // Picam HQ OV9281, OV7251, @@ -77,6 +78,8 @@ public String getFriendlyName() { return "Camera Module v1"; case IMX219: return "Camera Module v2"; + case IMX708: + return "Camera Module v3"; case IMX477: return "HQ Camera"; case OV9281: diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 7e0bd5c5c7..2fdde391d2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -86,6 +86,9 @@ public LibcameraGpuSettables(CameraConfiguration configuration) { if (sensorModel == LibCameraJNI.SensorModel.IMX477) { LibcameraGpuSource.logger.warn( "It appears you are using a Pi HQ Camera. This camera is not officially supported. You will have to set your camera FOV differently based on resolution."); + } else if (sensorModel == LibCameraJNI.SensorModel.IMX708) { + LibcameraGpuSource.logger.warn( + "It appears you are using a Pi Camera V3. This camera is not officially supported. You will have to set your camera FOV differently based on resolution."); } else if (sensorModel == LibCameraJNI.SensorModel.Unknown) { LibcameraGpuSource.logger.warn( "You have an unknown sensor connected to your Pi over CSI! This is likely a bug. If it is not, then you will have to set your camera FOV differently based on resolution."); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 2e6ada0aa5..970b1a6ae7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -142,7 +142,15 @@ protected List tryMatchUSBCamImpl(boolean createSources) { logger.debug("Trying to match " + usbCamConfigs.size() + " unmatched configs..."); // Match camera configs to physical cameras - var matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); + + List matchedCameras; + + if (!createSources) { + matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs, false); + } else { + matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); + } + unmatchedLoadedConfigs.removeAll(matchedCameras); if (!unmatchedLoadedConfigs.isEmpty() && !hasWarned) { logger.warn( @@ -198,6 +206,22 @@ protected List tryMatchUSBCamImpl(boolean createSources) { */ protected List matchUSBCameras( List detectedCamInfos, List loadedUsbCamConfigs) { + return matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs, true); + } + + /** + * Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on + * disk. + * + * @param detectedCamInfos Information about currently connected USB cameras. + * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. + * @param useJNI If false, this is a unit test and the JNI should not be used for CSI devices. + * @return the matched configurations. + */ + private List matchUSBCameras( + List detectedCamInfos, + List loadedUsbCamConfigs, + boolean useJNI) { var detectedCameraList = new ArrayList<>(detectedCamInfos); ArrayList cameraConfigurations = new ArrayList<>(); @@ -320,7 +344,11 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) // HACK -- for picams, we want to use the camera model String nickname = uniqueName; if (isCsiCamera(info)) { - nickname = LibCameraJNI.getSensorModel().toString(); + if (useJNI) { + nickname = LibCameraJNI.getSensorModel().toString(); + } else { + nickname = "CSICAM-DEV"; + } } CameraConfiguration configuration = @@ -342,6 +370,7 @@ private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, UsbCame logger.debug("Updating path config from " + cfg.path + " to " + info.path); cfg.path = info.path; } + cfg.otherPaths = info.otherPaths; if (cfg.otherPaths.length != info.otherPaths.length) { logger.debug( @@ -373,16 +402,56 @@ public void setIgnoredCamerasRegex(String ignoredCamerasRegex) { private List filterAllowedDevices(List allDevices) { List filteredDevices = new ArrayList<>(); + List badDevices = new ArrayList<>(); + for (var device : allDevices) { + // Filter devices that are physically the same device but may show up as multiple devices that + // really cant be accessed. First noticed with raspi 5 and ov5647. + + List paths = new ArrayList<>(); + + boolean skip = false; + if (device.otherPaths.length != 0) { + // Use the other paths to filter out devices that share the same path other than the index + // select only the lowest index. + // A ov5647 on a raspi 5 would show another path as + // platform-1000880000.pisp_be-video-index0, + // platform-1000880000.pisp_be-video-index4, and platform-1000880000.pisp_be-video-index5. + // This code will remove "indexX" from all the other paths from all the devices and make + // sure + // that we only take one camera stream from each device the stream with the lowest index. + for (String p : device.otherPaths) { + paths.add(p.split("index")[0]); + } + for (var otherDevice : filteredDevices) { + if (otherDevice.otherPaths.length == 0) continue; + List otherPaths = new ArrayList<>(); + for (String p : otherDevice.otherPaths) { + otherPaths.add(p.split("index")[0]); + } + if (paths.containsAll(otherPaths)) { + if (otherDevice.dev >= device.dev) { + badDevices.add(otherDevice); + } else { + skip = true; + break; + } + } + } + } + + filteredDevices.removeAll(badDevices); if (deviceBlacklist.contains(device.name)) { logger.trace( "Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\""); } else if (device.name.matches(ignoredCamerasRegex)) { logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path); - } else { + } else if (!skip) { filteredDevices.add(device); logger.trace( "Adding local video device - \"" + device.name + "\" at \"" + device.path + "\""); + } else { + logger.trace("Skipping duplicate device: \"" + device.name + "\" at \"" + device.path); } } return filteredDevices; diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index e16213000e..4a995d26fd 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -35,18 +35,19 @@ public void visionSourceTest() { ConfigManager.getInstance().load(); inst.tryMatchUSBCamImpl(); + var config3 = new CameraConfiguration( - "secondTestVideo", - "secondTestVideo1", - "secondTestVideo1", + "thirdTestVideo", + "thirdTestVideo", + "thirdTestVideo", "dev/video1", new String[] {"by-id/123"}); var config4 = new CameraConfiguration( - "secondTestVideo", - "secondTestVideo2", - "secondTestVideo2", + "fourthTestVideo", + "fourthTestVideo", + "fourthTestVideo", "dev/video2", new String[] {"by-id/321"}); @@ -61,7 +62,8 @@ public void visionSourceTest() { assertTrue(inst.knownUsbCameras.contains(info1)); assertEquals(2, inst.unmatchedLoadedConfigs.size()); - UsbCameraInfo info2 = new UsbCameraInfo(0, "dev/video1", "testVideo", new String[0], 1, 2); + UsbCameraInfo info2 = + new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3); infoList.add(info2); @@ -75,10 +77,10 @@ public void visionSourceTest() { assertEquals(2, inst.unmatchedLoadedConfigs.size()); UsbCameraInfo info3 = - new UsbCameraInfo(0, "dev/video2", "secondTestVideo", new String[] {"by-id/123"}, 2, 1); + new UsbCameraInfo(0, "dev/video2", "thirdTestVideo", new String[] {"by-id/123"}, 3, 4); UsbCameraInfo info4 = - new UsbCameraInfo(0, "dev/video3", "secondTestVideo", new String[] {"by-id/321"}, 3, 1); + new UsbCameraInfo(0, "dev/video3", "fourthTestVideo", new String[] {"by-id/321"}, 5, 6); infoList.add(info4); @@ -118,7 +120,80 @@ public void visionSourceTest() { assertEquals(cam3.nickname, config3.nickname); assertEquals(cam4.nickname, config4.nickname); - assertEquals(4, inst.knownUsbCameras.size()); + + UsbCameraInfo info5 = + new UsbCameraInfo( + 2, + "/dev/video2", + "Left Camera", + new String[] { + "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Left_Camera_12345-video-index0", + "/dev/v4l/by-path/platform-xhci-hcd.0-usb-0:2:1.0-video-index0" + }, + 7, + 8); + infoList.add(info5); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info5)); + + UsbCameraInfo info6 = + new UsbCameraInfo( + 3, + "dev/video3", + "Right Camera", + new String[] { + "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Right_Camera_123456-video-index0", + "/dev/v4l/by-path/platform-xhci-hcd.1-usb-0:1:1.0-video-index0" + }, + 9, + 10); + infoList.add(info6); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info6)); + + // RPI 5 CSI Tests + + UsbCameraInfo info7 = + new UsbCameraInfo( + 4, + "dev/video4", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index0"}, + 11, + 12); + infoList.add(info7); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info7)); + + UsbCameraInfo info8 = + new UsbCameraInfo( + 5, + "dev/video8", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index4"}, + 13, + 14); + infoList.add(info8); + inst.tryMatchUSBCamImpl(false); + + assertTrue(!inst.knownUsbCameras.contains(info8)); // This camera should not be recognized/used. + + UsbCameraInfo info9 = + new UsbCameraInfo( + 6, + "dev/video9", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index5"}, + 15, + 16); + infoList.add(info9); + inst.tryMatchUSBCamImpl(false); + + assertTrue(!inst.knownUsbCameras.contains(info9)); // This camera should not be recognized/used. + assertEquals(7, inst.knownUsbCameras.size()); assertEquals(0, inst.unmatchedLoadedConfigs.size()); } } diff --git a/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so b/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so index 90bb006943b0cde91e730c3bd0893ce9d91e85d5..c2d0d9b496acfcd3b9b2ba2eff0652f0c8ceef2b 100644 GIT binary patch literal 167016 zcmeFadwf*I`9D6p8z5YSa0wuwY=Q^`g(M`9pvY#E5F&^%2}sqCyPISof!s&}1X0-p zR4mqJ)e2s!Azn&SZBqp`)@lrDQCmyFTW`7vqA0eafMiw7@BPe~v-@PT2QRGoMzvpPA z(C?8dc?9ls?bDL=xu2)v>351tznsH0Mf9A1rIs-CzC}q+zw7C)KsqYt_x(kwh`B01 zBe{%KO!d2}j;z}#@Sx&kEmzmfDxhV1ZdOKyY8CN?l%5M-Z zalUnkRcx(!IIUBRjx8~Zu6-+<4OSuQg;;EF6m^Rm)3#;DPhL2-{${gj%TN&?6=M-~ z;~(<8d&Rzs2b$08*AOM_BW%|CEmmS2x7E{VYTVn{UyKl2b0WpSq^Sd=BA@RzM^+A~ zwAcOfPEjgy>pg8kc!bp?kdgLlSN9He->dF)dkNReihc$6KjL~#(RBM0u0P}Y3laGJ z71x`%-of=QuD{{>J1+kF2Z;A^btse zU*q}~7ytDTfrc^ZjRVq*=LlT=7}g)pk%}IS`w(116+KMdiB%MPCYpZ36*fZMN8uig zYYeWl6-M#KIk`9j(4; z@%T^Y^^9r#_n^`jzP#tNu7^v@o_g_N~j-@7?-H!olaGr;d*G)!MAZPhaaO+CK5LtGsab%U$QB&wTNm1+kBh{%GgM z$z9X|I740eY9X{&zl=wyJ7vmURn6up~3gP`NW^v z{W?bprBVXx! z_Rt;IKXS@ zCwC`Y@ciGe9G}_#_3tior=(}^j(A|qvkzQ%{GK^&j}|Ak)LirJfK6A=8FlO1+|@bOPg3yb|7!{hx7>_CNqepVX9Uu___`wZeo81%cjiQ&t6d_s79s6jd7F|QBTzONbR`P&BV zb<&{UEr3l$)_s1zw}#L6HUm8zGbw!hKO5BRs6jbn49dA4D8!|2JO4WpuS(jn1^fMIR^4~8}zRM2KAa{fV&Os-Y6_|!j)&YLAy^e z(DS*FXE;794D9^9267Mv`HnXze}sYD{%Bx_?~M*$&XWf7kgNNLkN=#3{k#&>r*LxE zVbCsP4CGmCP_MxT`Mzd=PewlqSDrrr42Q=X^rQX;cJgHd{k+qlzx>po9e;0NH>Mll z9R}mYiw5J1)gb-8uK+ahs!q@jF26{sNL{y>A?}$Np+6?-|y9V`& zHkfZT8tCD>2J`vl2KagdIlMPLyc|v##IHyBhxd_!=JmFQ!T)U_&n!en6ZH9^JBF3> z7*UmB+-@imV#)`qBa+*oX z=wkV=5T8z=at;;`pVEM96z+pPj1gyvTIGb)9Y}6zS5fI-X%TfiOYwXBSphkX2}7h{QXX@uLq0JaxO(E z@sDcM;-Pr*_is6l4;JxX_QtP;lE`+sOT&vH68il@+Tp>X!K={?+D()W)5_yh<6gmUayM$(fCxPyUNOq}oe5@qt18!uUtO55{TbFaEl>Jik))b>5-H11Wz8`)Tn#Dn8j$ z`VCR>8k<{?@{fm*LSbVw^ikv zq4Hgi_N8*}Z`1%?udc=ewY&2+4ZmO2_h+ae>FvG{IqyOKJ0S;OnpQwvp6jg|pN!vW ze2zix^t%}Sj`;W#AE<}?70GdDu*mtfM%O6(_s|pKKQ>q6|Ej|O1wEnidn_8R^SMs- zcdL@8YEI!(dfu+|+^YDrDEWxV8lP`eeWy&*%HP#^z#x)AR4;fK;#@aNXQVSNF&(`#0YzY5FF`C?xp4K9EkJj#Qquq#qd!t4} zjpgr{^v4bs8ILJI>ES=2&m{jGwXyeHe=U5{EZVajE80W$FC2=0YoiuV=O3%^#t?j_ z0iR{)zf{gvrMIg2MVcD#_J5`EFV=42Qq?bxHEX~%N^WhaHi&svYCi#;J}!4?Yw>wBNZPKf)V!YK)5%|MyhA zJj!0_dOmHE7QgZfE#e2NUj0xWDrcp#+exZCi;Dz1eHTVT0oZS?)aQ{R zUg@oF53>#8N2`8h3yFV@LBD%Y#y?9$ss5X$^1j4?|F4vsohP)2v=>dk2{QheK6>7P zd}Y4~>A!Cb(r}>++YXJ5xPFR?ujX$TDEukhNgu3@8lc21ey`-bRmnL?#qUN0itkhL zA6EDWYMecy#@Tp<4=}K|amtQWDn5F?QL3CLl-<5q@wpN5pnRPn@dqlsb%ylE=T&*y zRe53+pR*16b&VQF_AC9_s^mY}P}0E1k~(#Fy;1*9%6W_qbl|r|@fiaJp!yzD_0{!xj_MaarB}M1%vE;aytx{mKPf)X zsP?rgJ=f*-vg+^g3dgXe{Tz;!WfkR)>Ka#7jl&@v+4+|_iriK1l_k|R?yCID(n~AK z-TAJ9QnwZsKEzR2>vF6pDR-5YT!)BPEm@RTlU!J{q0*gQn3+;k;aFc%n_b%lINI{>t3_gU0suz znVFH8x$ge}mxTWlTxOS)ud7(?&di$y!S$+GGOJjQt8jJp_o>JCNTiCLSCcr)kyq&` zg!pRQMUKjvDo06qWvQ#soz3MNo?|?hXJ(!&dtK(@C8M;)=r6j>?MC zlEMv{i9wnFx3m4_p&B*$XdKSv2npsS{$Dw|g` zf$M{ z=e5__Vglea3jH3#u@g%2=yehQ%diB{hb{Pne~% zltMBrW1=1{zog8#gz<*s9~v6)7BT9S!k(wQ2xq5!-T3_k{q$t zy7d_(x8!`lVp^xe80^-9klStRGleS$Ky`A~j#pAeOW9B$R~iYj-n ztd^FqN5^pFx~tcgxe;e&ZLI@CMs)?v*=sg9*2RbGu(Ra4Ag#RU3aVb{Y)Dl;Sw=#! zd7q-bC}kDWAW79V6_o~zSXfA^RCL+P%QG8pK1662-uv#(vT!d;c7b-s!!6_18f zcU2h{tTpa%s?e9+k6EjxxG?8khb(A?Sb$c`U-n;~^~yY%xO&ah!Y@oi8-Py;WIHG7 zwUQG6=wZB?eqa@vZ8#msr23Ov09v@vRb5;|17Y^6yqcs+S4ma&qE)< z4#LruWI$rPBg4JIwYIcI3oMeMdQeUkS>!4=rO;QxlHHtO=^ZsyE)1yIiwoj2wJQNj zflO@%l8Yq()i|V!r6;0%y(JdxPO7(MW|owcP}*QfcxA>|5y~vj5n$4rgWi)}g=^N9 zR4G%wD42r^Pl&IqqNPMliMu+~O7pN}ns=1ilgVF@AXSF(>fLFRXu%?RC15zAxlMkB zOr47Xqh=0`j^L4y2wPbKQzv6&7ouVbg&5g&As3Ps6hgeH7Frmdj~e*|d+Vss>^3IH z)hKzmw6uCkn5E^QVrS<&u-?jqisURp_berG<Sk-VaGZFO;| zJe0^16IQy*Y4PRmH6}ol97%bVuJUX-F2?7{u_duR|4+ZO_+tri}dZH^Oe+~hm^U?3M)521P~s&X3zsvh^Y-t zXk69QNgXR-#I;!PJZiD9zJmdfvtVrsjKMNK5a=#nSyRkI`1c0sq^!=q8Tm3d&lA`V>}1Q{~n%X?oN`W>0M@Oz3P&%#qP!2 zxY+*-(z5ALPAEF3qzY+)L=sSBj-_==8>~Fgk8dqWf#gxE+z|0v5C?r z6Mc2nt0D=Ba`NE7JixJ{D6>qIR20@|v$DV>F}0>bO%UZMORM1EiU3atra5D^1D3M{ zI}}(0z`I_!D&LVw@k&-=x=te$hG`UP4u&{#xs-_N@=DBgYF1E5X`&eFk@=Alc3hR> z4&;HSEX)olLn)BiS)~iB*X3hLUtNsFzPk!7iD|z!w^@jFekpv7%9&Gu?O576sVOX0 zNEP->vA3cqH#|Ek6z$oUa{_Uu2I> zS`=w}DZ!G4Wv_GHpJg&G-Xa_I;NkXt2w%F4dIghpuBV;ZTY5YM`iw z>mBH$0c*K_$xpz16bqPS2lfb39OTn4mE9U@jC9(>iZ2g91M=BtpDXMA>EDA8osX-hqG!G8jLN_JB4q!>CTcwe@tv#2lLnt&|qq4DIvAAL- zAWg+od=w*576Y?WQaKa@URGi&OA=JXw3-@MVR0ar#UDkHecF{!{d?tFR~4F2Te?f}mgl2Y;Qpu1X@_H1 zk)ZID<07{|?IVC*TJ7#7F|1ZrR8*B=i(lI=L<|i{pxD~FAjpk)Yi0m}kT=?+s-%D= z)|~UpATW2W7AV&jh|RH_3Yb^umR4A1M<`y2dwpd^73o+yyr2{*qsp~jvZ3lNz{;Y! zm{4h?RN1PPo|3TAii#i!-B@bEQXWerQ`MWbBKVw0mY%OJc!N^2Yc6{w_5v!LRHfu1 z+WyWifh(dUGZX8Z%N&((3}6v}^AfJLwb_^EE;p3l2qa9c zDqr;ksFX^eI*#m3@#A8^D1%ODI8U`+eG-=CRlJ@yxtxYx_dY9mkrxq*vHQT0z8>hj}QF?=AcqnyQmU(@OP& z(y5t^yXXg|MQdhNuI)813bic>v(?cwHSa8{#jyFk3x}rZn?Z2o!DPDECRfmi2Xx{)BLQoR0Fazs^#Q@Id=8^ijtzBQ=iPT~VcN{td8 z>D_E?tqY$YY^Gc1+Iy7MQ7{W*ELc|It;K9@xg4l|7#_Ni1La9b#*|JD{WRCbVm!>A ziYjy1O%=I_U9fJQG*!W-lUrVNRxBY|)0}`sl@Xb0C-4oO**FM|^FUe+7&ll3oRV9R zf#PFYsaZi(0s3E2UNOtDEHNPwJ^@$J26(JtKxdU#tUr?!65`7%YD!jYaNvNvVO~t! zVFVdqfz`QWA@>QwsidY*iHQzvM1!hUx;B(nKzC^iBU_*5!OcZ;YuHPzM7=dbdYHaf zr$*<1`tY54w%*7omJ1G8yC1RONH$t3+Ovof*#WSiaW+c$FX7!1T=fH)e^$dk@0!c21Ut8l4PaMe2UAPVh zs;Xpyr1;r{R@W32PM=O`B)PH>Cl3M~3N_r}rjrY(Pyu$`q;=!Z^s5f@5;zictYfN2k?N%)voH&j`p=^75q%In z2F2}%FAwW6&G$a0UMxh<9$OUS?cc$%o@v3m*VP!+89G z!1Nd4%7y6um*FA1n^&w2VJUysv1Nqz8)sDEi=USNFr$pv7rL zuaV)j{q*muThxoE$F^zs zG4GT_K2affrNYOC;H?U`hTz8(J~aeyROL?z z!N;oa!P`P`TR-hR%Y`9$zQS`taN8g)zB2@mSNw}Z@CL=FG6a8E;k6;SXOhN0O7YX> z*}7E2ts%HjcuEMqT;ck6jdeac?NRxfz;+>;gmj61`g`D^xc|*gbu777CG`{|QhfrMq-byI0e;*_i*Yzb7*V{Q1-@(cm zuQipdS0lq?8Q#ip^(g=u{~3lW6_NNphO1A?NPIuT=^O0&?_Gw|_v7{75r*3sevIMt zJ$L=r&G5kt7wSE2l0%Ok8TUwrf6VYGhJV9wE5qqq^7?No!!sD3!f^WDy#8Cr@QDmx z&hRXTI~lIOlTExT8Gb&CU(0Z+v;N!4@Nabx_ZVu75+Hf_E`|0OQ}v@Yfih zXBa+;#cyZ$X%>HL4~8N9OtAAD-_PRH_xJVRy9`&K#F0Tq7=BcbjC!s1VOE+{*BFhQ}~` zAHydz{9T40W4MpuX$(KY@c%IU7{iY2E{mzbJ;!WcXNy zFViFAZe{p>hEHYqwG5AExRc>23}3-;8^b#ozL4Q-7@ouM4Gdq-@aq`vWcZs5FJ}1X z46kIk`s9TSt!4OKEWU@~Z!x@q;h!>m2gB7TZe(a9!yjewcQO1uhPN{OHw=G<;jIjB zXZRBg-^cJh4ByZ2Hio~;@ZU4s$M6>zzEyp2JU2zgmV5)j`}n496#q0>7;c*XL*y*ue1N zf!G)a8BTw1M*lT3e1s0-zKh{gS^QRpk7DtkVK_b^6Zo|=9G{*F{Pr;%pK=NO_A^}n z1`Gwh%W!;JCh+qy9G`Lt{EjdjpDqjhjxl^f0Hwbb6x3UM(kA#*e|n4b8J~m+{302y zKItRFq8N@(4F!H<86F!zVYeARiQ!WjejdZ)8Gb&)Qy6}j;Wmcj(>sCRLWbkhHGy9a z!>0vMA(k^dj^R#*PiJ^B!!KZXCByN_sKBq5;WGj#^q=7~8NQX_Cm7zq@OXyrV0Z$< z8yOCFL*TcI;aKtqeyt2o3ZR&uFg%&z?F^sI@O=!Q!|?qK*T20)!S6CWmBsfld@jR} zF#IBhA7l6*8Q#tCiy1D=!G1B1;gJl#gyB&PpU?2I47V}d%J4LXPi45B;qeSlXLt(3 zGZ=1TcqYRaGJFBUa~S?U!XTwJG@jwtu=ptq{~w0i817*BLWc9dUzfvh7mL4~;ROtLGQ5!C#SAZEcqPO2zmZ45 zwG4N&_#TEIWcXHwuVi=w!;2ZdgW)9%Z)A8k!*?-!6~kK@{x60G%Z)bQZ!}l?~ zjN$tkUe54$8D7C~AHypdeuUvw3_r&3YKC_+yoTW-BB=js86L^-bqtSU_}n;#(Qs#qg;NznkIl3?IqLpTh9%EWVB5Z!>%$!~f3k9ER^;_;QBd!*D0V z?`3!~!|!8wCByG$crC+!$#4(DFJ<-J%J6+GegnfFVE7J(pJI3;!yjb$E{5xW^Ob^I z8Q#d^Kf~~y3~y(66T|m0{9%UgXZRxwf0yCEVz`gtyBL0i;mr&`#_$$~cQgFg3>V5T zMC;$j7#_*+#~B{Q@ZU0gEW@8*xRv2gGJGn-cQZVm;ZHFmkB9L#?K=vYQy3fj)-C7=r!oeR32(aS(LG5RXd9gMyfbQhxwK$|VW{8xaEW%Mf0 zc1D+jE?{&O=z2!41Kq^v>p*uf`bN-QjIIZ5jtJ&|Gw4`G{}i;H(YJ#xVDufJ>luAF z=q5(r3%Y~R4}k7sbQ5TEzhM54f{tZ$3urr|e*?OJ(NBP`XY^B`n;88p=nh6d54wxd z9iYwqgZaM#I+oF|fwnXH4bTOQJ^;F&(QkupV)WlZcQE=r&|Qq~0&N};%>P5sv5fu% zw4Kra0bRi8FF@Bb`b*GF0lH!MSD-r>{VnJ&Mw`=+f25v&!|?u~V;MaNw4KpIK^HK3 z1n7E3j|Sbu=y9Ms7=142E=I?KHV+Kue?I6~M#q7+GkON-0!Al-u4nXY&`pe<3%Y~R zmw@hKv>mj0P%!@mpko<*DQG*Rmw+x{bS~(6MlS>1#OSL)cQE=|&|Qo!0Bs%|%zp*w zSVpe`ZD({j=mJJpfv#utI?zpwz7BK;qi+P=#prs_<{`oSZw4L9=%0eNGx~PW1&qD} zbUmZ*2HnKydqHlytN=q5%#3%Y~R z&x7t_bO&hj&|vf6^4|Er!yFix zV;Q{)w4KrApbHpX1-hQm>p(X#`Z~}ZjJ^?c7o+Pzn@0unzZrBaqkjt8&gk1g7clw` z(DjVI8*~$+?*-k#=m$V|F}ew~IXamCqo89M-2&Rq=-+@YVDuB9>lytN=q5%#3%Y~R z&x7t_bO&hj=wSY@fR1JKYoP6negkv?qYr?tXY|{kn;89f&>f6^4|Er!yFi=A1oQt8 zbS$Gk0c~gWe?S*7`U}wYjQ$dI6QjQZ-NEQ@L3c6QoR0j*2J`O^I+oFcK-(ET6m$Wj zM}V$p^k~pcj2;KNgVE=L?qYN-X!F^@{Lcp+%jh`Jc1F(tUBKu>(DjU-4Z4ZZb3u1# z^y!Vy%Qe#R!NQ-1by0QfcyD#A@cJwxTjurjZ12P!=cgKu54QR3Vx0H%#*ST9lQ<@h zW%<3|_KfM1z5wa5)}Ztkenl+B0V zk#?>sM<4l`V`TcbzaxEIApHx?!t6Of>)?BkMpz^B(jZS-4_hN*QfW;rW$VlG+l1J? zxTj|fVY&=#h%3Z+FQtjWeZCM!+l9!Ffeys%&+@yF=9tCQBCw98bfNKMgm^vnn@tDP zhSY6~`6lho7ZD%lMUUA($ns;~18L2HwBh9KG+&(xp2@O|m05ld(#Sk)>Hf*5d$zv| z{jds8uG2g|m2{N!lb8RWsE=M3y2s|&+8mK$jm<3b(^01gPdc`LE#88C3)<`vVtB#6 z{%u7jk)+Gr9BK1+4ikR(5_g-2;JHwEZDRi0dOnob=5Kno>v^2s7}V$E7SSS#ZEZ;r zHvhy?Vom30CAVj?{65R5mKb4cyB=wM;9dBH#e0+R95@e`8THwTYdGrXTxDycv@s)9 z+CK%;mg#A+mJLm7Mq1fM*97Cc^!Nw*#{UvwfxL!_d@1iDw9$x+R1?bCG*BePqD)lo zuFWEKiQ+XGi99zS^YhG-hj?a`8$I= zyAbC5yk~czIii>RsV?imbBmtlHZ6_wM%!sAw~^xXLE`&qPoV8|xl!6;q`gW{`>>Xl z>P%^0Mmo=tp55WyN@rOl7%jNuQq_G_f zmQU|PBrj^G`G|iV@}fSf_v_;rZ)E?pW~9QVBy0RM+i?Q+EQa!ee5rn&xYPJD5VpoS z12zEkD&WpM*Bh}3t$-(wcZcP`6l-eitG+GrB_ zcNdtvl-^~wAe%mAUR3H0DSE(Mi@_INg^!|@Q zJ?=z#DgF&SKI(l1=o64Luj3J5RL8rK<}29-5vf#{C8xFa=m+~dDJre`CfInqb>^LT z{g7ryl&Se4^waZ3*_tCp4QZZ#y{S1pQuxWv+dM*!S-PF=*<@wRogG*ZM5pQ1c3u z$%!#w5^Vo`|D^h*l*t7=d*|>97t#DC&!olVpo=52wSS?FOr;tVDlry z6|O;|jcjQ<;yf8^^FM7idB5y$^KUYbK8pPFYoO1&%~a-5E%P8h(rJ1&4fY)PGwS}n z((k8`7W)~yI|osJ7>#~8`gO8L)X#mG|B>#W8c*|uS9YbNt%wh$HJ$Fsr?j6#SE>JO z>MxKUeHgkxve5b$Xv_tOeh2Yzs&2%i)5Ki|@Jw;1eATo4uXr|Uhbs|IVS9Ue&_?{}C}-@jJ)UqBx_JTkJ`8X+EkANK8_A2wrwNNz`;=!QI`EzQq@o=3L` zr|{Y_wsaj7ZIj7h%o?|uUh!EaU#0qcm?9@wWPK}rm%l;79mD;qTlrZlkM?XmjA_7qOAjV zJq=?;jF|1sMBHGQtt|~@m17e6p731T?KF#R(Y3JqiDGwSmo2q%sz};7G%b~M>{`gE z4zeK`r6Zs2y=L!`LNnGzV_RZrPWGf|dl7Lc{8xxaWmyAy;sjB(^K6lCLm7ngBJW)! z>&rEpbt|69_8eM``I5rYqeRxkvDymXj@KgfVhr><~1(l5fky? zI5Wy`vWd1d)n_JRzAe&4K9!{taaV(%ZEp7*JrAOfpxhTyIR`Il)@5=UbU1k|y+D(P z=iHz?#(~E?#P?l|JkZt?h3TjV>y%EkRZN8NzCvNDPM0IDHb#wZnLB~vk89}wW}VYL zrycf>%Gh(c@V^M%1drX^7E)ZYCs%=Abg8Y`zuD~lXMxGP$r61OeJ}rVw7V}4dVaI$ zz7w`%5Be2_Sur>CSZw|yma|(-XrC{;d$u>B&m2O%DL(P-fqwpp>Wn%Zmv(t1#U0^* zo=}_DA>CaFzw>4r#(mS#7r*Jre+Rb8EWW+NX&&>sZg&M^RMj_MZWVKu}5Iu+YfwvJ^h=zX2CWh@18v2Z!C`OruZJzp$qzd2=jRw(=xW$ zx{J05gqe@-giZG$jj#%DqY&GCbA1&tsQs zW8i#@ee=hPwnOuB^`pZNy;V6#l%f52TF1*xV zzCze0%b25e(6Mg#5@OKiR`l;rr--{MK+_m{Hu7tP4yTzsx#qEG>-n$)G%vsyO5rw) zXQl~a#0MA)i7)Xpebtly0i~Z~^OKI~`Y~IMozX23;7R?0cUwEIAbr=5Qg+jJp7j!cQG9kM!WfI2rr|s{_FX%6R=fu4eIvy~*4NZ>3&O~@pQGXr!Wejx#xTS=DdR-T zaqMwwFYuyqtP5#QVth!(x}C5~k(TZ>F8l*w83R(Oh$Q zCagbigj{L-6R`2RZxi;n zRlJ#nIlDkV@#JzJiIviZ!IOZ~diGlItH)Sn#(I_7kK&pzUVo(IuGg1jK=VbCM>_6S z$a)iOJ<5ghd2&g9>A>juyeJBbIO;-Qv`agS`Q|7Z8%#&r;nT1QQw!35?z33wDBcJm z9vl}cf07USS*v|tmS6X;kPgkqoXLZ^59JSkO>0bJ7RE)>Q6I{I@}-V~&N_BMj-cO1 z-E8m~)nH5`+s)+?fjVHW(pvaZmVXP%OKa!{h2>}(cz+*VI%bKG{%~DBdV9(_DC$i6 zG*n(I##Y%r`WhNzyK|^oLl4!~&{S6L6L-lm3HC$z$!ToT_0N?ka2(e&;>2We7p;AH z{w9nsQdTPONm$Eco$$ma$V@=5cscOgXDx3-96e8J56b87$j6U-s9sp_%l4-_ovDmu zV{|@CzSR9IzYnaHrJoEk(tItZ7Uux5TcCf?y7@}XL)u^ur0gQZFzSc+&O+htZU&HTxM$SygZ`W4u6FW`sl zmw^9?DezB(Sx>hQ!1@Qgoc;Q)qp>c}H}z=#k@?X5#t1F0$I@4pR`B8T)YBVZmh;p4 z{`TfA7?UQ!PI-i7n+tZTuDGx}7Iuo(&6I}hmFDw%c^B>12yy7B(Ktpj{M*T%?a$+x_{g>f zU!B*1lPDi#OKpt0VazbKeEg+0?tCQgqgrl2zob2y0u$Cb&;x;Xh=s4fg7HW|&(I%s zo8Sw9y;v>BbJE2qTk{me>9h=pn;iMly^m~(xKGr&yRGoekZ#5zevdG(ZbbhDFF)nm z2|0Fw7i>`5hZb|(UZgAB5pmxO=;w{FLy+46P0!(fMIL%zqC55HL*QYC{_sBK1n)Fp zQK(C!=$_*=i#A=?B`=H%F$hc3=M0`m?@7o@+OpuBp_Tm1(;){wSGYyVDH*taCKmE& zy9fHJpNrMYczWY=%^#qA6<~ z%_d*iIM^oY@1upuOZ}YuViSBQcckZlYmk^`!g}}m{=%O=K=?Ne6@CZy@-~eVepj6M zgR_3~#m;*3RjNNr;SDXWIIq_vBB2c`ib4q&@Y->Y>~2!Zh?*2{Wbbp zLAlnaI^`V8{Ca`xTjrMQ5YLIZm-x14_gm*1V9`U&r zZF>&cKjfK?JN4(E0DBSgBt9`%!?+Q56Xg0nWIaN~O~IYYNH)ekK=VU6k;abrUZkgI z2Ymc#2zwCgV<*Nm;*~4wF{;Ib^gNHnz=0} zyWsP_(bP=xBcG9v<_l!Y5r=e4^N;^=*D2Uu!a1hfHL_f^1cK*oDKU5W#=|w8Te7%BG4A6H)?w-N2XZHZnZhj-G%FunGt2v zalaJTr*rz14Z*z(7wxr-@8s#}4=Qru_8^PY4+!Wc|lWtC}2fr??e@V6u5iRw)a}kYK+B%!sgY*MBc{zM} zsV4CGz%mMR7o6{beJ-?ka_!E6UW_|9Gh@O9yNEM1#|K0l}! z8#LvG@gd-=9#solGFt5Rz*p@XB$DcsulmX3J=@<#T}cNjamV=SB|DLTdIsAbz7m>u zWBl3(+^C)lzoVXuzoVX$zN4PSA@#h*pq_8Sr$+Te-wo7r1L}Fds^@H0PxPO#^-M!O zN%yi4UXT8&d|s^rzCPXOWs|-%oMXV;n0#p;!md%8xo1e@JV8FMV44S!rmI@huR7=# z>CcyYwefN}%5c(QYx{Cjf6PlpxAg2ar9OS5?apX?S`qt)q#tyq<3I=MM$baZ!wj2+ z{tk@V@d$Lt1|90$3|l6Kcz-MFJ`#R|Q7z9Pz7KlY34NvY6l`EV>17OjKb?pduKmA8 znv)UTb9%5hGZl7_{Ovv1pK)RBdJ497;=bgRL-2Q=K)Gq$=vgN|e-&#CAN+dVmN6|a zfG_#Bsca50TJt%<53Towy$C-6ziKypXH-V=cawcQNp(azTBFjQ7OhX-#ah;f>nNW4 zpOX3-aJc!ps)FJL$gh`&7UrEpFE0P8H>{5BsdVqMnj{E72PY3KD?N3sA>k3V;+SIu;%%S20 zusxu$cNtfu@rlEj8Fw|Fe+vH2p`>G@rH+w(zX=%G$j1JfjimZ|bZjhaq_V4N(BT%s z7vXGGH1JtL-6lCbgs_vBL#LsSUt&*P>h)$#uahVC*6Xc^dmMd%+9O-bOV{Vt&UE07 zL%d%Dr*nQJcWQs~S(6P5=rh`Z+V(=oGuVF8p2_y3{(Tbh$);&Gdq8uK#he*nvM=QmV7R?lOg z!__lsR9}027U~&~_*Bo=@T}K!n?XI_0!}*c52RuCy5YO+b#&YlNCP_-*NVGZ->Px_ zRXMH`->z#UqHx(K)z|?$uJ_3# zgrAs>u><1=wV6J4tU!HyNM|&5tV6oBxN30elL^9@UjX3X3HVx-1;<_ynakM7)!zV)Pa1;E5E+OVj zLD?Foh-8W@;78N(7~t)b74FgSv{*4)#*4tbvPJB^4r^^6_Qvmq?$9{W_@GF3#o7GV z+f82QT%6B>zs#3}y>0BxY{Xh>HvAPLO8iyg)3H{J5?&X45yWE;@{dB@;+EWeU^C9L z{4CKvE$-K^ec_s6o7Oex=G?90Y}4WveE3E7lACi^;_~78_vQ)Heg?nl8wmG|v$bsj z)-@n{S`+Rs;`-0b(bEQBesk_8*f)&B{o_evr)A?_jO(NEXHUBY_x=NJ&i!E6xM^K; z#!vg;A)L`TB;b?s9QgDz(PjpZqsMW62lOXji?)v~rnV!4O>G~IG_`#=-qhwhCE6Yx zYV-StncAE`wfRqtF}2-tclxxCdPLjd0j9Qg%nSW!&)cB~*hh|=IK;Ll4d-Rsc8c96 z9x}=GE1d%yig3>bp4=SVJ8{0F@iA}ii;zV-utmVk?OSq9;-*}}j{_GMdU7)r9;5Jo z1GfR654;I$#!l=(Q+%46Qu|Q;o*0qbgmo;1QF$qBCF0Y3fXe0?EAmsOi)1I(!(-4^ z*e8qoJ^0>?^c1IY295n=;(mg8QA~Q2w-h*JlRF4JZcuS-86tV&0Gts^#vZ|rhTNY6 zU#G&_=VE+U^}7{(D;4fp1ik6N`IQmg+$WD=eA9X5iDa6C#aM?!&&I^nA`R8!?}+14 zaT>4E%0%ZTB2X5zU2cb`J~s{bYf!E$RXm>yk^y2mBr2H=$-#%c9$gRXAcu#Q94=vU=mC##dYBD4=<{!~F=nhIW1PdhS%?o` zM?(ze8Lr1X9&%3`GV&hFlN{{r-$_UOl-A&uuh zz0*`8O{ku~gYxq>ZVYMTe<0jQ&o|FJbNhB9EpOjXL6fb}?Mx%iRFa$zUaQGjz<)z> zrg{y8j4z03EOF5D>yd>mmSw2DqFlKm->tI=4FU8F0SDD-f>I_vnF8t$K7RKKr)Dn zr})6g-cmj`soVPnYUqRKV60JzWp>{uNn>^k2Cetn~|Q{QOXnjzoGYe$Ey>&O;2@-S-0cAU$jw$|HmTo}EML3%Ee zP`#dva09(=*!vxNZAV%zkJ+HZ$zxWQk_Xf4Jji36l1CJi$6vuCoII$1OFI!^YukbM zG^pO^BCe5ad^jV>Wkct3Li_yRk?)yg^B$G2mv1_#E0Yq>tSetrotUnC23nVm3FoMH znoZkgQJ=p=*;e?b^*%oYvYAPFLswqKv#u*YHIU7lz^SfOA47Q@?JbY1!^q=qq(759 z?xivr$Yb+&$m40GmI4mj|7**T;mBGy2M73*_-DC66>F53_+h z7AtvBeg6N*V<^&}Ngg8*ZX}QN?~n)SJD0~K(BbAlYbR@SAnU~79LNTF6e@X~#!0!VttXp$8fcAmj4w|ov z#!&h>o?I_**8z=h7o`Pu8!)^J5?E_Fgjkb4L<}!%tshs9`-7nEty{(wUT+?0$5n)M zBE^$?qssFd&`!|T5skIzr=Z(Glf8AlVr#QLSC{({$txA>FJNR>1=f4t0&@Z@RT!;x z32#MuTF)&3-j47h6)xujjl*S}Um*_kH_kd2>raGxZnix~W4Ein?GNJJm}_Hjkv(-H zoW?&ci#I<7U*N?5Bre-U82@qi&>D4kaILyQotg0s4Zl{sgX)R27b7iilL^4B7khGN z1E)3_1=<5T5wxs(l&$Rr)ZGW{LWRk?17Cx{g4X4JEAp{k;>jHe%!)WSQCUC}f7csY+hD%By&c#fU{pStmlz@Q zx(xBjuWg-=^X&*P>~EU}|5zM+OKY&s8*Xo{8)p+!&UN8=6R?=p&ErfFmkhH46Sz}5 zPMcKjJdQ6|;o)qSS=xn!C@pFkVy z_`g&<@*hyWC=I?}An$Lh`~y}_d8G-2;cwee$7(|$C)G-jdjBG`a~P{=ke~-Qs~bfeD{a?rnIx| zBfMoe<40%E4#Cb6Mqx45v%TFT&0f4q(&k1ug;AV9I_orVdW2}BGvDZUJX^PE1rRLl`|0Iid`oeu*EV}u_FD}I%K?%aP;x6|R;9Eb0{fG`+hX$NI z&4RY*nl+B*Fb9rS2tVy9d~)@KX&&pY&{h39=)*b}jCm4{3hjiKr87 zhWg%u^v~=k|4a$+CX7uU#QzEOFco>xyLN=tqV4Ts?CY5Ceg3CWB7ba-*t5PI{nUK+ zwo|!yzwtfMoDUmtq7Y{Qv2TB(X9#@cmei@>(X}!}J{3XvT!8St@;N&wpD3fX%8=$x zwB16a@7%UHAfJmt>+<1il{;%S`Pj}1%I7DL&-F?^qe(u{AxirWc!ZNr4CE79zhg*u zruv;g_?hZAY51Az7Zp;!2!s0Ntx;>S3xf5FLH#mR{occ~UcUzo>i02lD|Av|)8|{H zlQs}GZ3>>>!2CrEJIi|tH0=%4p&wddLt`fh|GB`P)Z@U<{~sO)b~N@p;g1H^s>i_| z!(ko=HZ}GFypsp)am1ni@n<}lJBTr;dj(;RY?8FzJ%*{PhO+!QfyE!H==$IsQRU_`uV{lT)#_4 z{m8z^Hdf0awf2QHtHx}PJqO!fN?Okp`Ue>ccx$kMbkJ)YGyqhO@ zb4_?JjP^EjfO`=BHts&8k^8V6LXKxYqp)$<|HGa5(RgITb33rtaM$;M39kYz#(Q$f z-V&~lU!|aJz@NvxaD;7I|Cr6WU6%fFJr?1gf;~~}k;?r@;_)KN>OtHm5jRklSG2NR z1sQlIcyb>@7?p+Uej4%Hf&GejW9@9Z-1dd6*me?gZY z`5UOLl~iY|C$|8+0(E;stJ^@xP{eq0uSS?I!^aTc25cFyLacFh`H?J1W)TSYAe{OO z(XF7dK3cu=8clYmH_|!pbeuQLz+5h~*@^cQj_28NUM<4gZlP~ljcmCOayPPL?FV|> zu{&V-ycoydp&Q{+qYkj7CWxqhY=rk@wstiqf_FyESl`3ZF34V3o= zg?q*W-=^sd$!ER7ZIeXuA-@(fJ=@FmIPoHx>RknzbcggN4fCMuu~$#_DIRr0 zxmtYSUq1}*9zd^`SxnQqriwkD!I(>6AHTz-?c@8b-#|deoTjn55Q*hnhdtAYuAH~u4 zE$&zO#GNUh-M#bK34cWBJn=)w?@aT=PY~XBya~<|Cq*006SR3^f0U2U6Tbq#u;Wd< zHr})k4~{ojpl_dx_|%?n;#r?3{sKJ1+2i+t>tn3k>k63@|2m|ujUjX5-%?(^eD%S4 z?!~+E;eFFuJ^zHXyq@i#!`5?}R!@O_9eqFgUmqb~${Y2(m(}xXgL*y!oOFTeW27(I zdGnyYkdGs@?N=f_Z~IVrtfn#<$m5~!X!os1%jI!B=y39gj?v`d!CsCok9#1G=O71? z$1*05F$VH*DS1$RjO5|@ckljF97Z0Kkp4{an1XO4c^vupnlL^PTc^!Zv{V~6YHu&_(sV__#QX@ z+;m^R+Ktj*`4HxaPUZ_N0v+^MTBI*2rGf;n1?N0t*`Yz3Q zYV*ye`?dq8@r}-zzKXKb80T86jdAb`kLkjio&2I-A4e|!qR94pH{K%d`#JH% z7)$MTFV1YjZ@qgn;+z-hX{#e&af9W)akVkst^$mo_+HUA#BW5IAH?+lu3zH1AJ=`j z$R6HhDjGY@VX{Soc$W zfp_re8yFNn9`VUnsJ~-z3ePkLp!{e)Fjete27c6r@!%7KH7SM1hzM^4X!J$Zj?y11 z+mXgEibH-B0{;V8_w! zfjWTJ{lu*#AGAZ~A?7DmZSgnCHw$5fEbMYk?gtPSl$-PuOSydtez#EhP^PW8Q=cZ@ zLV!mO1i1vgI(<1AM5x>3+Ssm*XCgU5Dr;yGWJxZKMsvsqZb{ z9>|d7^=HNZD)6Ht{UN&;i9(NfIF3EG42a-QM$XeblUf2@Sf!= z(7a6xRhmV(b9@D8nz!i}y$?=n6neLl&U|A{9QUOq65kUrH9z_2FRl`4sPA}u=j_Ib zd7I7`Yo-mucgJowt$7vSDcJPDFiE4WT3wgFhy*mGDqUQW~#%h z{c&!N-g7L?@>gFsG0)Xs3?I3$I>ofml|t{L)kTQZyzJx@^5qBLiG4|)yL)<&7@mZ( zAiba74WN7-kUiy1<&gP{G224(&c+#Xi%9Of=Nhx;?5?9nOoRspu5UUqr zzmMv))!&m}-64ik`K!+rBaF&F2QoZ?e0~c(coX$Fh3hdqpTK<#!kY2?G~_W#)$vzE z4;91n6;1D5KRrSWuTeCe8H~a6Zd~`^`SeEZdmXwiMM5?nd`pGosDH}>`xp9mZ{x`K z`X#@Ndo&e z+Iy~_AbscHbn?5(G5S4~*Z;@fyTC_TT?^lPW}aLi2?TQGqC5e_fT#fiB#1J(prEJ} zsI`{UOcDYFLlTp4$HQ=GOADtbk#JF?j7lw}w1-+$I4`YHIX(2i=}Q%r)>Ma!r?hZ- zu%afKI^Taimt-=`sQtckzW4k6zKIU|dDdQg?X}lld+oi~zC6MHE#rKvi*?qJbDy7{ zXZ;a;X?KU-gU+jpbu-nvinbrlM!jm;6)DdKbfN&c8u@;~xzG13o@7mkgP=QYIUv7z zXDm5v6*%SnXYvqPNq=OwWeG#&?*&iR0@g$0%hbVQ=$}RYLR-#Owpn@G_Oy!lFRX{4 z^?}fO*i8E1dRWW5p?d|osVa5LZqDh>zy?cs1~WDvHkEhm{D;^Y zS#uItCi@e7&cR#Qm;Amz4f~BRTYQtn&|F3vSVLJ}T5?MsL!(DJ?{n{432lYX`z5Ek zc#Jb64dA@!%-r&>GxegI^DQEiVcf@B&bi+-+514;OW1oa>x}029R{`D2yIEX0@}Nm zOvZMzc5K^hd(Sjys^n~c8@{a<(>q=q+_CQUfOGK0qJe7OC)jUyocgK8GmHDiIOjBD z(o$$jIdtGsjui5l=2GwP8SMVVPkZR*1wL(F`hw#j9eXg0_3l#f=~G`m=r&NdtEt;J zX{)CObiBBm^UO2oi%wZ*kGFucW{KOlKe{kZH9f#Q!5Ocff7dzWqL;o?Xt4*_xKD2@ zxT_7>U4BHV!N-?Dy8+t6xH}`8eszdzFfugn3~wOaZfFeWY~h-I9WOS2vhH!z`e`+i{Fkfe?muaKYU^QYJ%n_oF6Ej$bSoA4q2^WRzW zDnVXu?m0TknD}4M1$K4-xAj41tYGT{`)tF?N!qhr7MZVzFY$SBMFtM`sUHOm%bo9Ys(hhw=EJ}|o4&}0eq`RK)UBb< zuGEWt{uG#Zh%~ySSu!P0be6k5jCb#nygbn}89Rahh{uxo59IyNffqi8PdnYOB#koj zmA+A)z3?FW+Qk04%$Z>7?=tmoVS_tw1c zk?|{bO=Oiuc~>CQcc>Serm4#&&G(V1q*06~C5_lSNf&3Olf6Cvi*_eAXPs;C#b;=D zpP<8Ha~AOY52Tm%>Qh!5+TBenwe_R3oQDMNM^2K54}767`TQ4gtKduOK_RbKz{zr@ zZt**vM!9|Hwv_ul@LocuJ-RT+upw*6cZFq>WPE4nB|0i)ea0xO=;@?9(dB>sB=EvC z$|JUc^DgsliSKxE-3s;oF~*yATa&xRUieGk;9jAHzWbpi_oEA4|B~E1xr0D|up!UC zWO!aX&xV|7!@)9p(%{M6xsiCU)6Bn_=k3lx$6uoSZu$YU{Aq@a9|8XrXWDVEGmW}S z-@+Y;?@Kugk=YL^x0FTNoOj9cJnxc;dGcPxyIoef1JAp}lP7Ic;wmJLF{IMtno1}y zbF!22F84Q|rB1W_F-^)H+jNP1O}v<QP*&}J_ z@JrlDy;tT=m)5@i4*AJCN*Q_V;cn_u&V+92DtNt+yfPBCEi&)(8uj5|T|1+76ms*# zx{bSs-X`tp`xxV}7sHS26?b=K;I~uo#d)Sq|B_W5UijY*e*%+n(Z5nZ8QekrJnJ}5 zBeRdWvuj0S2Ki)M{1y33lY26ZyHqk-6Ua;U@d-@MF6bjDzj7W&FSlsKiC;v~Pb_2H zBD$+o*CW)y#hK8V3U3?HAEScjwdbuRm7U zobvek)ny$H_J-=fJc_X*?=HeUp9P-xA=hCl{vvSW?yQaEb?_d!pWB?j+TW}3k%y#x zhJ3!WdU%HiT5j6Q#?_-bxaY3*46+a&clUF3TtMf=2Pm?UvWRRxgy*UJzQNB+z4_8r zXD0F#{g(bpY;`kXu)Ji?bHVIyvks>o(zW%*eVuYg-!$$h)v;aN@%@kw+J4SaBg2PY z=2^ySiZl}HRw(ySDaQTA{cz~WeefV{RPxU9B{YrUow-<}eYXnx3LC{=>VcnbzZC87 z2zbKh5{plrx{!Qqo|ViG_Y@g-3)*+-oip-cuFhSu_I-L%hw^Ul8YYeKlV#zAOJnC} zlnXqgTw*hzwe8uW&J^@O_^di(^6Plf`&tt9y8BsIr|@jf3tlnWEM+~A1<$3VDdY|z z;k(50>F9d@rZe=f--nK*vw2ZG+q^qSyB5=w-1xiXJ~g+d z9`C{Lxtqb(jn}z*!^56TV~mDx&X1oXFv5{PojUf!tBE3?Wy7^5@w51W%RE-TsV9E! zJi#d}%J&8WxgQoe@Y#xAH?4IcWm$pW@}pP9kDCE3y|olrX&L_2E%Xzlkuw`-xU-EmNzgXm(_59#(bR|cvs>Jq^3A_9LkbQH%?%4x=X&-WeB>9s zPHVkFD7q{>_xRe(Gp>Go9*Vu@k3&b+Dc2Rc@})2NgOL`0t%UHw`R!c8mk%8!vbE00 z?jxPp2g&Dm7Oq?F0vEc}@3)AL?OroTZ~cuyH?8%5TlA&;lKwB`ZR>~8UeQ;(y{72b zG>N@{?m?BvJec|{rH_(y14u7zIJm9KGd#X#-<#(kjo4hlOCzm}AB#x$W%KKz45d%w zPlJZo=f5StPSQ$#*9YZi=qu&XDUawp>pWwmtlx+BaAeEepxXvIt#?~jg=Zsu<_yDz z$h&=)o-vOY!8c48!^+smo?pnk;!~G4=4}6@(M{y>!8y)^x>Am3bltu!z_{qglju`_ z@clU}24`$(xMzQzhkhpme$(UK`-amWzQLH6`>x;r6@7{5S7D<1%^7^MPg6E2gM&LL zrm_D;=bI>svCPv>bzJa|F;@`&WM1O>67&rF8AOk7ZG@-Cy$~`!|04cYhw#Uqm+=q(JP=ZjF@de(mAJ0LGJ9~L>QuHtRO;+gmG{g7eE#d|HGjHR=nD2#&?~b>TBLftr(9sGo}$gj)&*%jB&Sf*X~oa z37z|5#HXV(cT~&`^v?nlX$u4I?XQ~2^PvLjMB1vP|Mj~v_fNg(um$RIN?j^=P$@#?`}UC54xGphHH;rXWOm2@j)&_Z#;ZUYCCCqbZ6*A5B>u;y5r{F zhEUzp8E@Caujr5PoW+{-mP_Z1x5Uf5)-J1h57-9PB>p8E=EoPqT1u1nZ){j8 za|;;{KSg<&k9LZlW--sx)wDMSX1phw@3L$>)8sE6=(*Wi294)Z>PqU=hD+T_oZSB|@j_>jm4}0GG1zrnhHQG#^PBYM zhoeVaFul1pJ$y7ay?ZQrW!Cpj+VB&|_f9;73Oy8=%APRc$$mF@)^m(<*f7c`(|OMN>a~zUn}fvOC1y5>2<{)nB)@WdGV63n)dVfbw24$e;I*Z)Sj#gA0T+)dieS@y)` zXL3I+a#rYl;AZCF>k{?_)^Uz(UDCcC;GYMd^NXz?A(OUOvYR^2Ihqu0$IJKjR&j<= z?(f^?SIn{a{&2aYiTTuP=eYm4oV0R&R`iAW3+ssHxWa=jdE?ac{+k&az2-E&!xP}1 zO0ln!&zt1&=2_XB2fd9Zz2&pizLoLDw;Vd}jzkw-ZKFQ?6QKL1v(t|czz-jCS9A_C zqOJ^|f}eNOmmuFu<~%uZ(oe1D4toEq+-1dC+4@(~j4}0a;@rH;eEm4jBKLhpnAW-* znc4Qx7>6<+|LdAx8@?a*b)*r#$nOPyXNZ%!DWF{pM=sJY7Fe=L5+4UV z81XX4M+V<*XK$r7zmJi4(h48qxBWKsqzr4}MPLgFFOZ-8ZC2@v(sen5>)6*un-cwS zc+#2_{xq4(%DnXnXh@twZwisEqY*#zNyh4Vd`msiz3=QlChU{D`x~ee;pg}8qb6|1 zmVQNP>Ng%Iew9(uzw-@W;YZqzm;0d0T}`s*QtZlG&<||H?{T(olb`d0C!pcyjDwEN z>!5vqaJEm=l``VMG3!RwM>1P~0iNWup5HY5w{F?fczZ$T73=`_GInO^>NlK?xTwp# zlX9Nfh))Q=@OsX=8s$yHpFz12Dc2e7&wp%CO&Z_QEVxk}e9slzs`Krm5%C51owbW+ zv4Mry13BLx3@<0oXVBsIkT>7#cM7hg-#dn9zVp;a^nyCdT%8Cd02vaQ`*4Ts75~ zG1lEMtg8fC4zZ8OqK!LGuuX^8v;MMy_>BX(qq6^y(QT}m__?>UfcTLT&%9f$b<8bH zb|2pOifTH)W$0)>_jx)-n%|r$1%5W}+R=W_-u5R?k(u-@gS+0{e46L3bLKwS%n|*K z@sHg9CHrDGflFH*bDsOU&N2rT-4iEdG8P6Jap|5)0 zft=PW^?ZNyS^B%Dkh{!nOV|rWJDfM%js9U{N|KpR>*n_;3pj`TtjI;$4DI@D`13rI z)U>udvB@uYq%Yz=Y0)e0&R$Pj6y5R{VSCvJL|<>{iCgyM(f%CJ&RncUJ2ldA-|Tw) zk3SP#EAQ9zFyRTxA?43}RqQ}!s}4+EW9pRn6-B3Bf)|PV0dXHwhr8)-{8m3I_Tppe zwd<`FFhM#@9-aiEnJ_0SNgB5=YJTGnZg||a&Ovv{> zM`Aa`x8*{%_IH66a<|1oY)cz*au_mo824|ucz;IvJ7E8TKK=o}zn^acsTTPrkTUkO z^W8_;e`?&n@iO=?e-L;+fUIRr-;1s}+}_u&fqyS;$w7NlP=Y}}+RmB{4CwgVh z|2~JzjdjEe$WiVlK2Kfi)baZ3kuQ2M9G;~glDgM`kAP?K$2hhs`379)!%`R2We({* z!}%T}WtF*>TXnp!2-r`k4=-zr{<*5NL{k@KuIX0l_*wE&tQ!_X|CvO!WdmiL2CuS5 zb2n|?C+m=uHywJ?$7P}S!};!)Th=96lRQ(Tuy3x5$M6fty5u2?|Fd(^b@Cd4oHkH~ za`IT8!I^OKaV5Hp^+_K(C$tO6^E1*3eL3rJ?rYd(WF+IL1Hk?L@7(A4M@Of~j{djv zeB8`^9_m=+BkPko{c9QD4jh3T#2iX-1 zGFJ2&b;S2HX}2Eo`&9VlyG%=oKPP!M+|fxNak6l+`OUR6JZIg12c}(ht^{BD9hW5jr{RpQm%L=H*v82PM|hC_So+`I-$Ic6F`6|lbg)m_IY!0>A9FXhj0pzo zTGOhsm?jw;40H1h;;y89!@Bz6a~s^c?z8^;_CecAAGGdS)wym%%D(m3EAe&t84m8b30!$I-sQs+a$A*_;CK3J!O?>S#Le%3u#{I(kG=Bp?HDh6YQ20r z#_QC6>>cC1^=%ea&}E_8ItH@3Hx4)Q+Rlt*nxmYRu ztbNYW`4qf71uw#HFhBm3*nOferPN0m_2D6H75ao;G_7E6C++tm`qypbC2gzRKcGqO zAvn)EwpmvhtqX^`_uW2J?JK|R?kKvfI!flN{Ie->`_9wu9~E7skL|t_9@~(G-P-~^ zk%PyX*|fYYt*MMLH*M(v_Y_?&clT@B`oZ@nB`S1fr z(?Ms-@pbIw@>=8A6~7Ja6h6cUb_Tm%u2Q$WN+vxsT9W&PW)(?PtId8~G~( zcS!35^w5JZpiSn8tl#Yf*Ym8ocSP)`hxt6RAdd4;qOqsZcCz<&u$WY9*q z8(Do59!2lOrpUf{AN3;RBzs@{v!wZm@`&GF(!D?^<9K;~3SY9!RVRY;9B`=zp)cVp z7JjuM>$KKQY? zMbsnqaTc(%ocS6?8%q(N2=Rj$PYp+B{c$t)t>*mU*ArfTEdQdMH(k~79&jIHFTMkQ zc%9Q_jL$XRM*`mnOunDK5Zlzoc}2x}(_w1R#RA%~pMG2LpJMDK>FGPD6V8>v!<(FE zoCF_IE-zylk%N3cTE3-3`!l|~H^R!#LwcEaiO)mEdcpji|C;DSW-Ib|?fk!DpP(t{ zR~_t$m9gV+g_*Z9nvVr9Q5Xc5wUz9!#n3e@;yWu0!6X;ig}(NUVb5IbJfH_%+oTMQ=~G-_=tGZ zXCQk(2DSba9I<15eq!I;%)K1D3pz8ICl1xjxnOFQd$i2Oy;;rGhI+Abb$6&@NgT_oCpN?{kzR-LZnYrNmM&Kfcu9xU* z7a@CmTc2)p9PZjleI;Wrs1Gl5-|fr+6m{Skh<}WF*=>Di;TiIExVOGGhR>hb zp2ZfGlQ(re#^Koemrs$!1M6Hv?nGWbWGQv6@cpW;cjKk5)>Bso3_q|9dzrQLbI=po zLeHjk4qAE8@sBs@;CCA%bI?gm#Bm59i8GAJ+rak@R_;J!*1~ln0dE~{LX+czR?_!+4TjT z-ZJu;TX@*}Z|bnWd}cm#%ZbwdCGX>sH@?>Gf=gZ*Mwro>Mz|b3Ka1XA$C_@HvY^vn zPiUjPsmFtLc0KcIX%Fks*(=O7U#4B012_2WZYpPYxSLX6 zxuBzsF|X)`trs5Ft>s+-7ua>!hNriSLcOGEa*P{uO=uW8mP! z@Gd%StQ}JJx!8=SsaFr4^1R(J@Fr~C-VdFUq?p5uh*c3ng+8N6Sa)xp0CFZnGuhr%X?G1B>Z&B*q zS?n8~T4K@lRSg~JVjqWtG3fADv$hl_y7M>OrIYXA2mf>y|5vga5EHKk41aaF8Fzd{ zUWqH_!6b>()d0DZrzh?@;#?21?mBOH4(UkS6KAHon|o1*yGh5n)1J8NjQp0oOWbVI zWd_HY>Hgsz;yk1q8XRZl_hZ(>hu4x0etKw{>HfW)I4|i&2FID{{_}0(8cCNO9A}kJ z;(VkV8yshq?{~xxZNl z`f}#MTw_SZPvB_%Ik?7J3^qX49DXWz%-PFV^mM|AI%q+VtGxs190ha>Pg^Dc9Qo$%MjzSYwE3h;R|=e&n;IO`L94>DhT5Hj9y zV~f&RH}f(Nmo(|{C2NHqSpzZF#{XsS@86WO?yh|<_N=d9O{4?AYw4W@ayIV$|6m;D za5;`IxUHb`J$$BRtc7HWzm#(qS@=N7Veu9pc&4Nk`Lf-VmKkX?+o0?~(V5^eyVLEpx$bz(1htDc$Q<{h=ZG zK%g?-%qA~UIK2^5px5P_07BsA5s_IL97uBXDkb^Udn9NlQUoH$j_dx zVL^4Y9G^-C<@pim8}RE$T{ZCR*^u6}9G>X|X{+h2lh=BFmd0~{u~GmZz(?de5ImhR zWv{$gi#R*N%uC1L>tOu({NKBuOPh}m4Lag4Y83kTZXS^E>GY=gebmlE|M)c>G)cG0CnTWo~K2*^c(VH*S89BRZ>5QMfrnb&h z;?EYHNtX6RpBJh#^vj0Mw4pOEqcd_ofA!OeO)G~dH4UQg)bQgMvQK;(x*Qv4`Tchk z;D3%iehYdne*Z%Bc&(fta=8rOzv=haE_&EQSk8RfOPPif7kYN+BC=YKz9CcYd2l}` z^-5o9`1?h+@;xU9-*a;ISDnv_eF)OkW9ZFJ(#oE?mIOIFH1wjemY{|n&mV#B(crD% z^|ko+r>UVAIV{Clok7OgAXyL7k%#E6*M&cU{O6O$qpl(J`|(Z8F+05vuIZ3AHVC-D zWUlc#dAXVIDdtLpSOb{m(k^DxRt^6@W!V5fBV0p|ug51Ke*Z$sBI{m5Ex*5)@s;@e zC4WW!>yeA?2RTT3KjSbDzK}<$C;6tX@T-`Mc_@$g>hWD{k^V~fQ1mye(A~50EN5k0 z=2@9B#6N^=;t8cqN&AW6S@cla@h^$9pI_zKY}01@6I`=Ba{o8{nQ_jhH%^)D{eK0) zPFk=REIAr9y@|%yP~>X#W%%E-Xe-!-&ZBSj*k*~h_Vyge0w;LimW_W1y2cn5Kb*Z6 z=Rxe1q_xurr&GE+pRsnX$B7IX|L&)3>xQd*JFV=i(Xkz`01unq%{sK~1xYaXiV42# z`}Z){`J+`bF-u`bnk98AuA^pF79;+ID{-@4k4I{7riSt-Z@O&DXe9X^_ zyyZNW=&$I@?+B&daxA^*sV{TCool4o7t5JM2W!frbN90kY6a~={68WarO8~TXj?ts z-LB`1C_(OevdHD5zp?vkMAxK^uY18$< zq)jiEHHCrO9+P}K;4?*~keAD_DZ+!4yD#~8t@DO2L1!1|4a=MvTT0pIs zzV8(s+mM^lM_+5&81Hb^xnY#D?p`=*>`M{)in*enxva5XNBQ1|u8iY?&kv3jo~e%> zdHzac``LaBNkc-&1K}}*0n132$y$|sR$eE&Z_zYwn z{S~nz=$GhQ2YVguy)+-P)*V>m%>O8%@4j>2VBOZ^?g)J9%y$qzQ>Hpz<|pe=Zshy% z+!^fsKtHi*9;fP*`zem$cVe#ozJqbW8TNUW7OMOP?04BK^ht|z^Hyw7O=SaBXXo5I z_X)n7Ll^v)xf@((Ke^`>x$lC#RGg0}0pHL1are5*HyG#8*W5_n;uA5x;Vb@M#{2@m z9=L~kEI>vMhp#S!`ltPj!OuC)f&P0q6Jp0D2gPN{9(cyx$iPVxI@7(V}{;7PusYf@Gzr<7O9Ddn}x`YY~Fkh?DC-P!S?m-`boamM4% zEraJH=r(=*m_P6?esA9#b?`jt#Bc3mo_6+i&W|yl z2>|mrWUp6xtLTf&lin0rk#;5FyZoqp<4*dQxxYiDetXU@q50b&cot)nGYR*MHgw1{ z#L)M1=j41wx-l+x(8rDg|3pw459dI1?hpEK9lBHG%s(t;qJG47mDt}~r;cnN{m+S~ z{oK3TiC&TJJibVCETayTC*Y6a-Wu8q>zX}loLA!bzA3u?e6s*Lx|q+?^*ftfVZ=MEmE@Vt?KZgJrppynO7bMR#%Ft7`9(k68l<2;b*tw_87UELcC@fxlq=2$kVkDMy61R>Zw$pV!s`Qud&>_5ttV+S-nhW?SRT zT6bGZrpyy0Z*=3|dA8fy_bE#_8Or#nYaMF{j4_rnwrRoNDZcPQ*c+KM6s%d*A@VuL zoZyw1Z_il?ZQ4Sve(Fi+_Ds^}%XJ@VgXQ`J?_uRS$g*K+ zJ#zi$-voC49(jsfR|Ls5kMe|*>jtyTtx}#1(l>GknqO>Z&>d(#{H|hu-Rv=NF!tloi1MNL_$Adrxywe@A2vWg)uj$+ysh@hTvYaL>RLgEv^O2Sd5SfG=jn5^7!Qm9 zcb$v1dqWQxV=-=#_8iWS|JOAqjPa$Ul{5wT%VnRe@G;YnA!}%+EXVu+8i_oM{)q1A z%t?cFrxU)VKEsVsK9joZ)gF8F=2gD!^Lg!Y2>Ax4>&vtukg+@5NxdL3u{t0O!)n}XWt+MoPy^*t5lIA}@Cyk%=4Z*Q> z;4Vwo{G^GbpR7Ig(lyzCA@){uM(Ht4Z&02N(tphFBYq$9yTb1S2@U;8Z`FAYS09f7 z7u}LN_j9*eus(U|k3^qDmm0wn-S~*SWv(G~#iuQM1m#(L+H%Ih%rC7~Xu3!v``m1%1_>9U#0BR zc!NBiwqVZ??&c?LPv$?jS@~B%Lwp93Um1A^`$@C>Lz=E&qndxh9{d#c;~^X2c`j+a z*cQ=srmlvr=i+}dbe;LtdKu@5ZDU+mdy+Li_%FqeD8AEXBj2>v`^eXWpDCG8&SiPn zCt>=nU)}ppubDozTfvQ}i`a!=yC-{Dz7BrLNY@*eS+7WrEzPf`U(%KTkoZh4VBT-{+sb9P87I|+2mZml)L9H5Y_#+u(YzOVu9XJqd&B)Vp)Y=S3y|cpnBl- z4W)GrdU@&6in>x21TQUrXjx@lMR$xza-LpQS!N*?nSgg;}Z`JD?7EGB^Qc}Kh<+yQErj(V|SC*I5H`G;DFPd7uxU}xJ zTl9uiwH4K5Z=&*8R#j0_xo~QBP)-Xf8YoEplqr?(SIkByLnlC-OZKNb%=*=FEDktS_jmD_w;f+WZR^{1mg2 z>Xwx^RMu4M3+rl@8ocV2^?G$pgN|G)7g*`;u31n~r7Kkc?(zz~w63DGehP7avmSJp zdCHWzQh}zP1VQIilsA-CFRH5e|Ak82Q=^v#iB!$0X(%;n<5vAT?ne%4hemCnlTv95 zD;HVy($juT@G@E|&8nhwsfo}NUs_dFQ!eStmMvUZQCDxqFGt!nb+pX8OGqRrC6(3n z6?F|3{kaX}@=NB{mMkrOu%e`%hFY*(2Oxrb`Klm6wLHH|&WCOfOGpwwp@s?N2{9;-;y@<^%ow@-S1c{7sGB)%LXly}@2D#+<2{VvEO|c= zL}=37%0<t!UaFrTGpFX|YG+7*(fscbw^0!2zN9Qm3dLwWf|0?e^rP{2Y zJO{Uv8hR0Z!P3%_#TBKsLG7no|B1WNh?8`8%+f3B>T2r5zEV88NGxGhWp#yq-GV84 z&dTd@a#zv})$}ik*wfz1DytXh6^p8*z0ypq2GtW^PJ5#@(xw(v)G73+yF;{EqoKql zrOQ?dpF!zrme$gjTM2cegEfjUMel`2As*Cg>uMI!gN0%#wWx{`&0Jc#sA67e9X<9u zIK+00FTQvBT*(i{iwp)0oV#Ys5g>oSiy1f6)vW3rQC?atT?Ul&(gh3ZD(dU?nuYoo z<)Tz|4gC(PY8oZ_(+4YSYZ(-nxdoTIa%pW11C)gebcsWv21-@Uii)~9XpYI^>SUSXx>;WlE`S?;uoDwM;B9W<>Hq)M6?n#6sGU z$gdcl7u`)kkzon2Vc8g#ysw%m?u5?k6g-_z#Y$NeaWsB;@>N%rtd06!-e`x~)y)rrrlU1`0BCsEh zKbr-Z%eiTyK1Rmar5 zt}r7aNPoK{%4%w=^lnA&21lx3sY|MA{NnoMREvzl4a&7fU3UX=^%;oM5R@UH8j zjqWDV5KN*5)D?nI*FypYs<~;^>XkMLa7+v#VKfE(UT8Gjv{JtjW~#FrZh}O^O{*l{ zES-r}4HnE-+&C_Wn7Rsdy4sLLc4=8XsaB&sk{B|QnzCn4qTFL3e}i55=-DF%#LWTa?j@`jpPD>-JVJ|u0B?!aTZyY1PI0+)+?&&D@;3L5e-Tgn#f4;aF|AJNCzt-b^G}S zD1!MhjSqH_qMEhS(+H|Y^M_UX|3dqZ)NCW#ZA&5>hWAeO<&3Fr=#+Uu^bppW7{>XXCL3c1KPxAFfL@Tk+D;l_m8x zB@0WXTcjaVGAp98x+el7BN6nSXkLR}DY&-~Ox1G>gX7ZpiI1Op{8F#d08NoXNVJu%96m70iPdWL#Iin4Omm(&#L9L*eh z>K82zqbp`tZKw*`Vm2tt21T@PqOW!j@nr_7Ce@l{4YkV}aG%k5CqhB`!5|Gc2M|_6 z9&zr)=OLq0;rOaE*$(1n!x@w+jPD4ZHpQX&Astwm)Z~VFn zV^RvG>*}ZIC3tx1_3RakE6W#)&+xj{%S4I|i!1AIysf@+bp>wV-mxkqAH(ZplQj6V z=8^W~Se(WS#NDG;$uzYN;#7!H0S0{~gR7kJITLbnb0+3Y%9)&#my$j!9unPrB!$%Fjz*;&Dh0NoDi zt6cNO7!YK`Bme)Vd)}Bsph16we?BY3VSe+1isd)k-Xz@g)yr0nEniw1K zS@94SEv_hkP~^5$79ytT*VT(xPN|yZ6KSPNRW4mQejKfc5F22Gxs!p{EYBaGH%`LI z<0ldpcY87`4Etu*Z(Zfh<)w8Ci0}2DqtsW8WhhxUGyIC!)LP74J#Hu?`=EHsWZhAC zH@?K>mF1?#sVBX-#>9(=FBOZo%1o|iR^zi^onAD4dmE~-clH;>n zAhSLr~XTJBGM!Vb8m&;r$4Q`4S=6 zMaXx3)KoTLTpplQJK_BUl^QxB5SX8?)O|03} z2cI(n0rp9#6NJgoZ71Z6xJsNE2;>nCC7eY#me4~ui?D%kKH+Al$ zL^z93B_l6FoiLGw`<3!ec!2N{;RV96djf%Y^lU0&En&-#phq}%KlLws5f0^j{*MXK z!%Kw62o;y~wi8YzR4MR9m`b>nFpuyA;Vi;-LJwgVVS~IMK(2(B*l3WM3jCh~fvto` zUPk}Z$oB+tBAiONhj9PDBNqvO13%<f-@lIZj05#*0pS5~Q1k9Tp|_Ubp7SDID5gFAvpZy@VgAG z8U9BKFCc!fjf!%ek&QQ|H&aAC)aP$LiU$3=N5U{9!h^3=uuahJ?=V{CI3I zq4rNSlpY;ub_A6}@}9--{t1DAUvQfZTqFNt$-ij3bBE)rWc`)Ycl1ks!uh0Qx`iY1>4Ns=Aljne z$;d_YWhKvcS&-}9A`z*iV)LO0Q)kGN^ARc)8Mg%C7j1LSw(;+>@r%qBkqe&ahfcn^ zQnu&dq@bVYiNq%p3b)7ah%4UKx4F;GSZ5gt-|CTtw3V%-n?YX5l5Vq+ZuS#>pX^h( zExtLfXnX9AnBrZoo!au#ZxwWlvEOWolsLhh6$ZXj+vSqDuk{U1m++O|uOhFpno_54 zr$3Rh@+W$wiwx;o7b6fY-r&jWd_sS6SmCy7o3AO{K6J;BqMd_x4SF>5v5dmU(_7M9 z4>%~<+wH&o-CL)7il?M)Z-xHe(Sd-QXIu8zk~>Yw2~Zh$&E$C*KD@kZw;6dFc87YJ zVT!vTK_~7Acr)%W+rBo+z%z89*wO*fUq3j-;4})YSOdrXMEsL+(k!LE3%Akf@;8;= zSo-7aZv_IgNdtcdUeR`KhpT8?Y;#P}PN%bgRNa+U6Z|5&H49zI`wsm8^{72dCsokT ztUsw9X)ABMX=K?Pw=G`q*Mh%!Gj?C_w};@1wFts524BWM@swpM{Au4Y@V$;FlAr89 zeOtfgq{2tzAB!v8p132SXlLJDeIAc(i81)?T@`NVu=h0)-O;CTXT0;y;Nl@ttK1}&x-`Z@jAa^l)^0Xv7B54=J+vD$%a=(5mc2?}&v3JJa6+0t#rqIhF`3Y#9 zMqW~v(CV>m*cNG!BokC=1oEWZp!w0c-0#wN6KXpF+HFy?E8H2oE5>M*g^y~Fxe6b5 zu7J$j!Kzye4H>tMqzsBsD>rEP98aV^nNqZEVDo?nNMdGOxTF8}w1NSgSmOTH?$g|P$#K0#96mCmy?q9sU z-;SixT?soAE8aR?B;hjXOXw_m>oon9)$;l^Is{#?cP8#iko0$33CP3EkA3_qcPnjD zbRt+bw~Mh7`B;)^aa4$GLff_2h+^vH%G^MpgK>v8#gvcpiIGo^DBL!@Ijd;9zGGO? z&TDsFQ}pQ2$A%O>KDcF&>z!~$LTvc~_&NW5#;WKh{21-0NK9Cf*tue9Nkxx3A2XN< zt|zg(q92QwSZ%$?(8(ShncH1WJ^e6gM6Wc)*`7wmAJQK9pkOcEDwKY!lEgj6tQfpJ z8KaZe0x5IAD6`jD@Wi!GUQ@VjX!DTi+XwF$RI)2$XJ)};X^*DYw4^+q>TGzsefHz2 zEh*00w@&}|cW<|s?abVj@o4&EX#%|a?e;s&$hX^_`!LnN`)$$h-?8h_>lCU&G&qf9 z=e)0P``ZNx&XTv=izL14K8FdIYi3g1%|wAPQA9Qp7QWqXycdMUa?Z5CeM3?wgpj*E zgj}(i?v9XTCbXb8RO!gx$K2x7h{%0HGC0OO<{xRpJjhfmwF4Xq#3XGt&_>!#OKS@|1dn;2O+)DB0ZRsE z%UpP#1uPTTES~N0mN7QwRno}3at84P#-F6|@=^?JtU#0v-4UpI@BL@PeXU>BaB;k z*2WujjsD4~2V>A?^c6+jRslRomn`zM)Ajvw>7<;CLG2=)*f3jXP&5pKDVxp24`nWG z%Vsw47GQbIr#l469&gA{L&hA_cR6-Czj^wtAY<;f<90fCIb;xXC$`)+Opp+LJ8AJh zAN!;|KyC=g=&K4JO?)ijcImLTC-3OLv)`_y$NRSQah?fd)I|~Z;q2LW^XSJj&)~zvQa5`iBKfjw>krQ`280Fu}v-WEk55}L_oevgx zX5Tj$ftZzVw(Zm8vzC0^q$Qm_FTFEZue-p>eyK<1qNB-}ijlxlB^`e{zmdRlfEjgX z(UP%dFfG1YZ}>if({bm70SityeVdbJY|f1(KQB1B)NydSW()Rt>BQ$Ti*#;i$e+*> z|Hn#Twhjr*53B*0=%D;f<#!6b@&fZqoB9q7Ay}?PJvi@2x^{;qHEKT_^2mj-=NFc3 z>XEHcBQn5}JVZabTCBOs^+q0k#}h-J98$P5eOFrHw!zJVineF&$S8a?)tGMnHfS~= zdb*PIC-4!8Pf#0Xq_^f3GNqUylM8!VA+rkSpJ6g|@FDu zZV8cbm@$LoUqza|_?s4kZsqUS3ZIzu^8o4o|=e(Ftp5y&EF{vrm$n8Y~6;00BMF=yA1VI#C8 zot9#hrDxt*C_}FMf>fkH=AaeK9?^4KZEk<-v;kIzFK8b04!mgkQMKRWOe|m=Dr3(g z8H`RB>56b?>`2<)?{*oqK7@HMJS`?&%4#-q=S~?E7YI&);M^gvMcuE`SBZb_(9o;( z)NX&E7;_a3|JecL;unCW&-4TJ!FZyRiO6p;cHkflOshBeZgV`5`DDf&+f#R>+_5da zIc?{_T?2|9O@6HZmad&A|8xPsX`&8nmbO?eFeMZ+=+@?|`%NYi_d1WpT#8eT@n)_|lIP(EbL??k?xT)5wXS#^85+-B z&OMI(ebfO>+e?I&!}F$?Y$8s?knl=OVn-k4i`5SHQTt=Ht)z-A2GS8{q>a-+-yE+2 zIUTQ^1RlTADB~u_P#J%naA;iVaM__7unOi+!Q#`At{ zkAoqsKSpCwpd%)S=Yz2{o71t`#J0rkaqLM@C*!pfebt5d9G=_z2&IF4HJ%Uk)xbZ} zR|p;JEAZw7v)Ef*Ev`d6UWoHKHr=3h_0haHr~`d8o{#p?+DEJNeX@CO>8tr}P;Gs+ ztv9F(eSMCOF=|($c4f3Wkf`x|DG?!!QNDiKsWIvh3N}Wa>^HSzv}*64&2wXNHb^Iv zeU3e2xK&#_Fk0;zn8S1XKw8+rl;VS9RCDSc$B8lOSeoV=qx@+a&ll3NA+a~z=Quh> zozBo2$EeF0Q+Ym`>2sVOqpl3nc8yV62Wvbx56&jVslh(Sg)wT=P_1Q*Y8k5Wyk}@O z(dUQy9DcB_)wYgN`>)k_K6q_5(O0hZIWB{xYt3WS5nbc?sGd!yHRbrvC7blc#pPLt;`fXALJO2cfzsB>1cFnc<6l608F(; zr}w$GIu7AN@S_)vH>l=V$pIDQ`A95?UNqvwSamR7yK=ocfnJPN{`fr(?`YMGGF-3r z_tkhtF^=4zP64?=b@bKRvsFuiwl!NFNysL_E~<6&4eCUac6pRKpQQ2Jk+jFrLhAn7 z$!vABzsB>a{$5-ro6@qOcQnlhhyILg;LVxYBsi9t4NIE`?Q!_wY_PUb8CAlGX(o=;ty4Q}%=ti|=zjCN_1YUkhU)p#WUcVsq5?IW{!-hUnPICh<2Hja`~H;)p&ca0LEw2TsF ze4|8gdq!z+v3HbYynmDw_rNG2c5syBcwrP4;^g&O2ane?x;UDxoyt~S+-If^j+SO~ zaGb2saQVC?u{4z?$=nkZFXq* zgO57o`I1A!w|T^=op7o1PU&a%xZ;6t7Sk!Zx6JY2SOyixFi`zeBQt5G+FaV+evVVx zW=CUxbv{-*&|e*k1KrzKJKrB2iU%dZ>*TpD3GmS*0iTq|D@pMr+0#z|mxbA_{RN@F zcC5eJES-U=)sk=WVfBqZ&w=~K8LnxAa-A4 zbhlrV4zf8$Ap0@B1JspRt!*IoRy&lU_Qz>^Qq-xqxzU%_l1 z5Kr2c0rI?OpvDO5$UvcXDn;|9s!J&X-;}ByNL4MV@_b(Wrqo?}yJP+-rw9P0b80+N zPe#lPSsl;Q4#laxjz%Qsb7>bi&yQT<)MZVcy|IlBUK-=5!M$+;KNu(QlTuyh_;*k=NW47vNgu%;7UnsOXDDQDpuq9 zQmodJ;b@FYB>F&{wt0}~1J9>1JzeQ)E47d=vf%k(AIxmJI**}AS6zKIo;M{3^_GNU zQgkHv9PJruZ<4ktL$rtI%SjS(lDTv(ErVpt!m}?q8-k~jv!SthKsJ1w8US%P zOlfhP%20<=wM*&hWU9vV>C|jUZB5H2U0YfqQx0Jgr045NQh2}C6jl|WPiQ3*sP5S2hw0#OM> zB@mTBR02^6L?sZFKvV)z2}C6jl|WPiQ3*sP5S2hw0#OM>B@mTBR02^6L?sZFKvV)z z2}C6jl|WPiQ3*sP5S2hw0#OM>B@mTBR02^6L?sZFKvV)z2}C6jl|WPiQ3*sP5S2hw z0#OM>B@mTBR02^6L?sZFKvV)z2}C6jl|WPiQ3*sP5S2hw0#OM>B@mTBR02^6L?sZF zKvV)z2}C6jl|WPiQ3*sP5S2hw0#OM>B@mTBR02^6L?sZFKvV)z2}C6jl|WPiQ3*sP z5S2hw0#OM>B@mTBR02^6L?sZFKvV)z2}C6jl|WPiQ3*sP5S2hw0#OM>B@mTBR02^6 zL?sZFz!yp2@)i>#`MYK~+X@@4Fz0(Fyw(a2Sm9+W^jYccd@{S7#-lsNVTALn(B0R3 zxAAf;yhba2j}>}H^v1LCqv8Kq2^{!pNWFx2xMm0Fx) zMx6-)8}n|Go3gh=BEO^KsdFPkz9R@3h|2 zEIR+)!3@i8{CCrh(hHHmTI)R;nu3VRAu0!vKvWN+dJxrv|G9QxYM1iH+9M1Lpu8_B z^EZvq{zZh{QB*WV&n~Vgt1PY7$Bn;f{7pGGj>{cwJ=!$Ghc4w%$LxV>5Qc&xI=S-s(L${w%vhQ}*=T-O`kN7>`q-f#waw*3l< zaH#}k&;NqJlqyl#xjyj{?tBZCB~3!0lN> z4;3k;rrrh))r>tqCce*}2r zQ@zM2n~r)qJlqok9|oG}gU8~h#WIf`Bl*?s(WZYOgpLi*c_w_mCnLc9KM0Q>YB$P3 z6FzOY+e+p!!U5`XNV_)SNowV^-gdx-H-?3$C~ru6xAEJ;;wPyKVc{vNF#?@TcQ}4h z)ZVc8DXJ|3+#eR6q&7#OpZRHTyJVIN`xX|Sq}n3TaYw-SN5G#Q0YAAQyc`xsz`s8N zzBepB@(&A7QvR@XlGNo0a8($N&rEeJ#6EvZDdoU!+i;C{`FoY0z)xEEHZ_k#V*fXX z&@tit)$DKBmH{t*7{cIy3ujcJB9zBdBC`eSc=o6dnB z_J*5ul2u-9Z~kridn3^I{wW-tBsJqJz3JHbCSUB0{~E7iABy)|@E9|E--iFxe7|hL zJ;p2cAp-rJ|1$CIe{K8=5%6pOVq==2&8O!|c=&9~4{pb%P)y<9hP$l@j}Z<~9TDtX z=A7R0G4V50V~AemgXTcrYwa~zXxuXN2&j^;C>6=hA+Uph@M{vfuFSC zmqXxFTqb@;2>gc@T*dLgpG|)t4NB<8hrkzD@Z=EqK?|N40{_&4>mhJAdLi_)L*Sb& zcuolX*A_f41a7x?w*|Lr$77VHzfv9>VgS@q{Ij3euD!0___kiDt)bgm&^(YMA=O$zfZ2d+)OF+@MVY2#b)W-DVe-vLU6?T<3m3`-6cx>^Svk+TiA z>-T#We71#u(n?^rC4R>1+ z9wQu};vnnE)X-0B{mzCzZo%z! zJ)6#l7Tj;)yDj>5d22(;n1<6;P(2Z#ZLD)U%w$dKegYp1YiZR=#S91;ZrTR zS*`&pIjlZ2zMa>bzKNfrj)fgxr6_+`IICYF>vz|~k3;pqziRQb&Vt+PefL@L(-z!b zFTBTsoBU*|ypVQhw=WxRm%_wPQSl+`J)>E$bEqD8vPFNj1-I9q?e(kIEV#YiY{PB( zp?-rKD5}u6;n^1bjTZcVOa3<8q=S8{v^h5uTm=8~?7;o^^~SgLt@g&=aFc$r+O(oK z+{Ql;qCf5AEBa%@L-i*!g8n3j_-z74d6Se4A8W~BAPcCX4{i(JZto_anaUlZer>p2 z3KO4xHpKrk3i{#XW|#MJNO{MBFXgr2BP~8n`U8|6w!E3DAcRkwp93NM-0=nc7TgnpZ{~Zoe`%40AG*KdVc^m)+kU6s^1RwV_FV)2YWM+*zPAww~aP@2A(?3R5IyK9{(yryKWrYn@^kn(@T59heV)n^M83tZ+x51t}m&# zHvY*F{OifrfuA-MF478D-ihEVb+x~DhC%0Qeg3Kix98Djd6aqxxY2&Ca@q5p?-}r` z{h5sh{A&N_4uk&tflz<%ZUdgt-Cl%i`TLOp@38~6RDWsF3H1+~{L;yX_|MzHA83;7 z_3xj750`%28b8=}IF$}79RAn{@canyk_hlx;EeLG`g_E{XWklXmeg+*crS2+PitO$ z-h#JBp#Nu!j1o#aR;9DZVPep+LB?4Sa7k<#IADR-FYPnyB0T=%5@w3hUEe5_Vt)B26 z;Nj}A#-bnU@7fpv|A!IipNIhOiU9A!#xJ8?`%E5jrx<@;;Nkdu3b^Qr?U&BA@P7+D zoL+ru(YNO{_Pps91Qiay0=UT2_MhAG{A~pM&mzG4u(TGA{-_A>NfF>P3^?{@pxLmF zFi)1hjS=v-0T+2DTlO6FHvXQAfdAVF@Dw%-ioU7q%zV2{cZnJcT;y;2^RKn&-(%og zFF`?F1o*lL@a72c;}PH=MS#b%0aEx6U5C2Of`|I48h|H@A1P$Kz1F}FDv@e}|_O&Sj{y$mxwtv+gPyW$@`x;F;elt)>ETBrg z*nW>}3qBfnIDU$NhqIFn27ZES`?X160rpq^_C%o59swSshtKyq;KHZ9p2(~5_jMiw zZm*Z+S@0)qxHZqU>-{z0;rRa)c)0c$mnHlle`_3m!lIvH!7n>a#ouJX=NNG7B`Ek- z1o$6-8{?U$P2?F?nrSFuIQh>8F8H>;&TZjuwBYu7`T`3+eT0c`uWPqh@b3W+NB_|X z@SjJ3|GP!s?=uPSvFPs|8J_-G1J3yW5fd@jX@;E^{J=-%d$C0)=eqE8<^vZw_;~O_v93 z_?FnzeHL5~fwuq`xw$_z@ojtZmH{_m!T%=T5Wc*61o-#}aJK=!y1v#%z;B8Gf5D>D z*kp2PnS0e40lyCl7EW(7flK?c_r2KdWifCWkJ1*BT?qb(K3L3XTcHgb*1Sk(JmV5!g*vKW3&0hMk?M+3lUj z4%5ABV-a0fS5fUqjJ@>rtsqQZWPV-cCO;3wyRhE9N z82_ab_{$~me*?Toj@~V4|FK%}xIYXy+0*ONZayUEd6l=vdFw3!Q1tnA3D4{E7XhdB zzH&pfe?a1SOSV5F>AWK0A2*8UbEX9TSPA@G37l~_`1x%y;7E@9rIPl43HbNQHu~jj zCGc;S;CZkvHf9epL^K?dGpr5|65@*_HN= zp7;8=Y@=U3RRSNCz-J}!7Xjz~q~ss9zdr;6NB&hluktA1Me}*Q1in`ScQ_pKCHeEL zT-=vR+W)a^e@5mvo|oX*)aP~iVnz>E0g_e$V@UIPDH4u>9)8~X+6KfGJg z{-Z0!eETrqTpm9p5PVV6;V}-EAG8YtoYs}klYG9U{qM{6dAs%%z>D<8R{^JeQFVsT z$+4eq7t`(M0WZSyhrB)f>RSTAqY}@5O1P@~eNw{D{J2<`s>e$tJs*|u+6~cA$>R~= zMf>>Avc0OO{SAr#)z1{S{|^p_zw~v{@C&m2!#^SR<@z_X<6Z`w@*b*w@*&y&S-?qO z)#SOqg5ThHWFzhJio}2J8v+P*!CzmO@Y+9!&(BNvBae&usPnZ?Ncc8~%MaScD}jHm z1pX@}@ZTwczr^9+Rrzl@e@vUd-YjYVzp{Nz%JmP*_Gf-l()kYr;JSqGbGZDVU7iJ; z_+QmYD*F6cN&7cs`@A0h(Vr^jhn*6*TLS+a;6?oZyCv=ax&;0&9KNV0-vIoKwtn^Y z!E=Ic`S|55hpSfU^)n^#r%T{tz`6dE`ByfRp}L zb?G3Zc&!V%)g;}{NbpKYT+l-MB6>c-@yO2Fr~i< zt;^?Ew-}43eb!;3F&heWReLhQD2v0iB7to!98Y{Vaf^pC4G+D$$<_*EUg!m0f{%tV zwywD>9$AhX?eA{d`h;-1&Tmhm5OWygcDKedEwb|+`fL=>STc&-coh0Tna+9Xf`2id z_0n6dII*I{+}rFXOEcF_!l>hQt&V4!reQoYwH!9HqR2YJEH7Neq8AXf6Hssi^Kv_(yO#&Pdx;+pl9p>JN;>Wr!F!u`RDJ?82oT=yWcG;`1F;>D*MX{lJr2 zbZaS^27wzva<`fj%fp+j{+0G(Yz3IzT=yES3vRON`(n(_dOS0YX0|CX?m!Kb;XZHrN zajy{9?mFVyJ)?!feD{f=Fwd!BEVGSiU?;@VBP(!xu)@Zy(4jH&jIq{);q|(SQG=5a zxel8o5typRmIV8NQLgyiIs?-fHMY$p^O}3zq|H`?Bj&(>!%0{LQyMF5TktjuC-Adq zLidEjmM{^>n&O80wEi|m^|+*QhbCSAWzYxf%#(I5b32lqD}{1SwQG zLNQ4_)$87UWnyCecNec*nLwN;Z86uT4!q91yE|Qj%{_XPcBj>`=Ng;}NPw(p_ zy?Qqh3nj|8>aOBVC!FQ#5zL?uBTs5382ZZo8Vx%P2HtQrNg}TwW23iVNfF>7Zcs2k zb|0hQ3K1b)pf6`G1CUusIQHzA!o@h54hB`bf&j@KdU4`LEEz+3!@vbn=7enMhkeUu z4x*x%SyM!uGPZN7wc-|RfZ-UAftZVs^QbbQ(1_7TU}AbxXKbVF6luxaA?UUI~5D(4||TtOS(u^fjrS(D;mwz0Rm4RyY=&L9iy!p;?DUX;L# z>xes;6Nl1{RMwxUM(&`>*zoX>L8r%IfM_^5Vl!H&WkvRAsmVD0CbR5kSmgQ@zSas~ zMun~a%+Bg|XAP};+iUYtRnG!w+6k{K6cFyD$3HiaD8NKEh%dz$V8YI1o^eG{woIu^kb z43G`1VxAr#;vj8dwEM_XgcEW@Agq>6sfFgj$V&1ns@FIgK@7O!Ncups)ov}Uye-rW zQV~eyAfM3|t`Z0hRV`G6)hcF~GQiPcAx0Jo3FDVSqtSe}o@ zU2Kn?7)XTy4weNSlMqQD^c(>X-tBaoj2yqvK{yo*U1*MYQF7;4+!O<eo#1XHf7$zL@A}j`uco>EyeH%uzr~yb4OV zheG#rX$Q4?-ELBE=)4M-7s_hlTejQLRDsbt4`z0@-DDIxyAaII>=O37ra2QPk!@z) zna?ZCB_|oox(;e4uEw%l`AD^5bOA$+Pt{X!a!7JKk`4|k|>`ttpvq`L8>n6~Ps-t0oR7qk?*=-AH12{daed9nS)vm_HiY0%+%Wi^U=VJF!z_vB+I@q{04zXTY95`3h*emooQS(npgmoA5XE>*J&2_ znZs2oA!VwRVymtYMOrzD-iggbmZ}+h)`U~#2o(zBFHCA-*ejKr3Br@!;C>XPP{!88 z9QOsqj0U2lgfx;HMPUT4K|(#lkadIB06*hCs}EgT)ft8iv{j5*L8g`L`RZ;5o|b(G ze-W;0fFp{;9-V{Jw0ge7jB2Ao?xHR+!#+G)?&?74&pg{DVq|kiYxSZOLAuxTU<+(0 zG#t|SbRZ$=T4aWmZJkFq(!vubWocd~Ea_myrg}x~C^yJULP-(F<(88ANC;L_q!~?r zo?ISDM1_{QhqlWd37wXtHUi-^#6pK$3A{lVjp68M$&VsMXw-r*@dif>F-300265Sr z=EAl2V1i**I|KysqtRApyJsb?m7>uI3(;QHM|K z*$h?(JJ7Moh|w&Rhhmd2U<>nCZn*__4gFQ5>t?QWS!vHwZj|yQMSGXqUEPZ}1+PgV zQwA$*Dn$g8T`Os>Q)yp~7mc=%t&ERQz?MP>K|Lc{eAdZw)4RF@nIeX_`rgbSN5w{H7PACLvT%|Ip{WxIs22S#$+!h4$5}pyl@USn zRm-2c4bTyR0h=3mehA#(-JFfdkkDf0Y9t&&4!PLowTR@q$b}UzQrUcksi+-;SGeDD zRTEERSqlrc-7zW&I?5``$^srI=nTh^lzQ{UOv=qyI@P&HWwg3(5Qk9@t_rq&JRxJ% z#})}y?1=N$i!w!ad_KbgyFkpt#y#tRQ@u{=Hg#1gW5NRWfC*Kev(@d~2G_dY5E_7V zo>6nW*bZ?%X7gB5`LmskK7|8KoWdai;2~h4(7VIqYCYoNdQ-+0DN!=}R~!@>HIcI? zBFk`H+dqN|mF$Ifk~3Z8M%US8l|YzSM}7!@y^rN|mXQeB#Tg$A4tbQsyz(@y@HC=i zktHjUi-nvTY)R0tL!qah$Hs9?O3?)dBzxFn+ue1x(L)s9Mury0I4wHHDLux(ZQ8$6 z7zwD1c_s&BfKA;-*~;29ije9goSKT4*3Hlo73cz6jSQ;iyCMpc^C&vk`Zl5@Uv}An z*ybG$#58gg`?Jd;qeWFoU2p)8xw$^bDH`rZAKQ{h)gZD}Sy5HzBdAqE5)OK}#^841 z?S^j+bDAh4bj7-BU7k&51BrRC0***IUok`-3DK#m60WpxHL2YAsV0P1W#Ek;*W6;FvB%?TvI^ z5J1k3uYDvrY`w6A9I!!|Ja1zEyMq-uS1SJ8bo@u!=i$r^k^$ci<-A$+gnC=y& z3rZ4(yCa@4Y@8}%*ec5ywpeM#kl!lK7y{j?GKOlyq}7JQpWpg)b0wEjnVLrm4hbXS z1B>J$ao9YTHCj-I+#}<)c9z5O%A*Q#zRpxen{1yWQ>4h&B0boFn#VqD%w`MeZX7`& zgAPMYX$sw`#^{TF*!wKN%L+}go&t~ONd|o-_!xOWI<^=^xrwGUO$E8Gc$l0Tif=^Z zmKuY_)u{|Z5xO>yBG+ZkZOa=Y9?U6`j1nkOV)C>m9pN;t-^o-oMEb&HittF4L1uRW zsl>TQwXD3UVqd8;Msh1l`rsgYh9=l}-HVa>raI1ifo#FQJrXJywGA3v4tQNC!v54xEGcL)d7wWE!&oM+!95}im2yuy>(lJuin0!7 zx4}r~^E76gP7qXeE)~us*03_31`AZr<^oO?JctC{+@)7DC^+6CVsvnP?D4ppsH+@R zm7~4S*~^o$KUCWy`{&fO(MARaXAnaDs=T_&5Wt8ZdbuPIxNwQFP;^5OOPQ)7E6EQg z8gTCsUCP!SZW+z)5`$rEFc~1oD4w%84jSh!htYuI=ROBN{LZpE1^wJg+}G?eBCBn( z7|8CDafw9f(ma~4#K8pVtYnbY6q33oH&-18;h4FXeh)gj*CMV5GmU$e` z@;*d5s}(3uD7B4s*Z_EgIF>xNSDGO7lLib3sK^C$5F(f+5^S8Ie%~M`7RCbX?9K)-7fkw z@IHA_2{$e$;ZUwqRFb-l)Hw7Uh-i8+gjZtP3Uw9(RW(neW3dI!G}jh^3u~qh*T)%` zp=zvToL_!^4Zjy&UKoT8cC~KQk@K{iBPy4&uQh`3Ag={UYf^IZzN$f)A`?zIov{lS zLCOX{EVuxVUSup_S)d*gTB&?`K|zi!1QePf^A!#$HpOrlXxAcHfC^qbYCr%=Cl0*E zYv7pj;7mPsL;5u4;wXx9Qx2T_V06BTP)yWxW81|sRAT^A8^{e4KlDP8*E`*j20!YW zQ;SlKK%F~WQ@ZUwZ>>^>AQJ@?xrzgd!gYs}TVbC0PnD4#JRMS2rN8dA*k0>scoHlY%oqR|7SByQ%AKU&x{rwsHA+q;)OByDw^bj z=UILs2>uVIom7D7Ly%WsG-)i;ITCyS8Hh!f6QJ)NNS3B> z*)-a`IP=&<15c;)lx{{^JfrDaQL>04B8OKG;R3}U9zmxg_Ok3zQ4(ezT%0U9fc@#I z=aZGkW>TpZNnp-mHqm%3oE(Y6yV&gWTz$qqpISt*Hj$pxT0>*#6uOgRIsA?21-_t#Q!;vR)| z?3LA4O{$OOr+|9F?k*+XJn;MoFtJflZNro6*K6{zTSB{)F2 z6x(y-DP_>t*6=U^x9$4e^lZiwa-+}(;#9Mz%4y}U6?y}7^rhkp5phGAaB9lx6;U>y zr)r`1N&=Fx2r7rvo>w=}y7VTP8a>{W?#H)>zW~piKObShAwWenqM%?X90D+pMtrGj z>YkIx-O4TZ8W^tZg=n$lE5Uv%l>)RdQc7JY-n@hv8$dbp{bJK=fXtjT>d z{a80;OqE#_F0RJ0r*X(mw*888xUeiZZ+RwIoU+PFoGR`QjL$scoqiNBofz=dWZU4w~A+%s{N_wJCAuzg@2-F zRVr2IC_qnfO(Jn;hNt7G29P(C9~kbTw$P#u^4-KwDph*IuTtenrYb!chs+kF2+z-Wq+?3J6J6YbSi(OE#nT-AHJ z#`x3w&wroj=R~WAzFShtIEM(?kFw|jp5ifSpY`To!4`k#@%8?UbFdjJ&{ zqP70|WqhFV75&f2{_6S(^+kNLQ-1tk%k}Tc-=$O6=hVi@{`v859<9H)lMN$pVjp>+@O}Kzxv(7n(Y6u+8BvI^;7u1hBw5!s=xaE zt&8u7@zwY7s&%XWY9Id#ef9Cx?{2;Dp6o9!YPdQ56`nV9{cqkD6TW#{bbMY3mc*d? zsq3v=|LgL1j<0`M;5jGzsf(Jo`urZ=5H7WT^}WBZ$o}ekS5`fjR4DPYZwL;l$(I4xvL|ZS+*@x zmTp(Xwj8!?t*oRgVq13E)~w9hO9NV2)@FHQD&zZno%j39ycp)#<*|?b{QmfzI-Yqw z-?!K6^?toy@9R0N%A7gXY%*Ddx-8-uVZv|MItkbmZjCdmUqaZ#M9~xfo-2lFaeMef zy8k$EE9EAH{uEjY`ENMke^B9nkog}_&yK!YGEv!2KJ|1Ii_#_BC(|X|C)4TAd`SxZ z8KIJg;Ys%%nXKD$q>86MDK1^vAJ-Jp@0aJxgu%}mr8)hnr@IpAh|iyTMXHE7syrjT zj8>WIPbH2fTQu?@cCu#A6fA~>~YSSRD=WfOSq}L-ngO6 zzE-imW~9j^qT_m*MgGkBwt8e&XB9JT4d&G|t!W#kCycVixYn6XYx{|W$XJV5J#?$< zK=z)Az09L})JKZ+0aL7XYpvpCA>v)Gh6L~KhN+PQ%#S7>Uja4qjAUMz8H4`?j+p$m7;!6#4{E5rMNG{eL3#QxNq)r>Bf|beJL-j z9x>{ZU3>4i^LhKIomXGB{i}}(pBVV|TSF(m^3u{9H}x;8y!w&@Gd{knsaNTJWpBN^ zWYMu_Z71G;>h0)PU;LeEbIONnci$czId)6tJ$>(-`*DAhXYPIdyn}b0J2h^Z&04(R zkrys}{Kad`gPwih#&N}yV&1!^|B6+)x7=9!uUDpx>PWn3((%iOzPtSO-}c@&a>^xD zb5ES})q+dn-#huzz&NYtj|a^UmpjM*_1!1lY`(qli;FJ%=c*a)oA0qdf3VNJpKaQ{ ztmlcOrtsarGp#sQdrRL78{fQabeQY5CGX98=MQrWGoRgX-l}RbmS^h542ttkT0UabmV*=5kGTIHdu~(WBfmQJP0#(O3QDs| zGVh=C-K48sbCxwec=@s06W)1oU~k97jMx5oS5waA^Ix2?=xBUZ{cA7gf3~sxnH%hd z(|3eDP<_+Pp&cK;GVRaHHa_vyZ6CjHy=nG+cYWEJy5hGZMv2dRzw+ro_dC0CX6-7l z&i~V^rr~wZPP=yN%IhXXdp_Fs+JPiTd+)Zpnr0PE%y_sssp*-2pDuX$ciDgVeDRRd zO;NA3nP1JByY|`NnG)xm=zo84-zlF=o!j}ysjUfI{AmM{$knm@UcHkXnyF9 z{ui%qZNFW&7o(>|^b!fnyN6oDU{U|Da=a>dP9ki*dsHz5U?{vKE;N2uRA_u+LTLOy zeS|nWM7d3Hc%jO@$$+1&v7zJ7Lgu0Jc^mwMD)(g)=Vb$Vqs|Q-|NJqbafd-YUS)tg4D?V9K9Qu`^{?|nmm5Abbo@)Lq2n(_ z=NC#pYYgnCmqEEU41Pk@%asOt-fU2=&p;0^VH^}H|FDZfLfP$?2K7GBK%Z}296JB|F>wf$e-DFttTK?R6iy~oKIa8Q60U^cG64yGDn``xxlq5`%KDH{k#C;i2>IWe~p}lcG?3jx+FY zSv^C?|JES>^_Xyl%Kt-ydYNmW&)Ej{Fx{Zs2!niT(OyFF(`umqI}F-Of`Oi&GqBH2 zgZA>KLA?wx@DJM!{9%THy*+8bf0jW$Qw;hSlR^JJ%%FdH&!F6$&@=w;cHLr7U-u6V z9X}bB7^)tN4CFd(&>y84lzXXxT&oP~#b;m-j~nDa*MQH72KlEN#2;l)@AU@d?l!<* zGZ^O_Hz>C=EVTX`4E+Di2KhW{;NPYiw9|Jm4m`V?^$2m1fgfu#7*Bp^;D=KU>hUoH zxfUCYOCLTfw0@$;gvLi0)C*RPLfZLy1N*$#K+iP>`D``Fr{2J?_A-zw(x5%&8tDHu zgZlcn0soPxbBNIGiZ|frltH@~WT0n@!T97H1AclMUMi;X%{vq7;fW!;sLrg~n5bnKC0&tJo^}#68 zb03jUi;K8wRKTZbcND*Y78-Hcw10(|dx^?N;k0f^*B9sqW5row^pz5*lCZcLdZv6L z)#7T6Sq7}u>={Pnk@R*IzvUdtCrV7bS4NCc_y(QNp)&uAR6a9d5S0JXUr8LzR=fVF z;y)B2@nnVf*7Vs&>~%}vdDuN&55aDU|9OQnzFuyMlB*+I;)teQRK8YjkUmE%xoop# z{5>lEa~DWGSJJ{Vu9*tohIUH%WGj76Q+SD1Zj@MdPggx$rS%KoU*$vXfv!IA|CCR? zTIimp^wSr5AiQ#rEEmbOYbWY?3*_y{k$9xSr})avCd(V*&A*XIl< zoa$?ts;`ME{&6L*P1P6J)2=@%y@~rJt?PN6*1o|1EfUx3w+Q2N;y*#fkC4G)tI~tE zNDpkA6hr*!e5q$wFA3=SdD$xQuqR~(d1!BRr4Et&X9wBaP4Tka2BkMzlc($N+BgLE ztQ4d3zXot9wkNx>Ho`-lAqxA(yI9J zs{F&0A#Tw8HvH#)$;d^@u6jWK#Q$35XLNoJDf>xKdaGCbFM^>`JKCf8f!ej}?=TEa z4?+HF0m>&lxV=1$RHV;_ApNg{Tok{BHn(u0*=koLDwOcT-%EP4!lxP7|0vWu`3-M@ z%;z-~|4U>@`Bd5@zDMCZ4BE?3h1YMD5nU?&Fjc?#s(zK4#RW>AElLl%9ln8niuATU zsQr$H{gNK6L4M96|n zy~;nwsr;A2t|)(-@{R3r>Z(`TcM=#teb9Lhhp1li%= zP%m0O-^utdtN5#iDLYJ)!1qdkvvhx`>P44pni>agKP~gAQTZHF_O@NwiEf7zm43GO zl@ay&J*4v4Ym#_}lK0+=B%i_Ks}B^P!FKhc!h`#TaOi>BSHdka<9RCo$50WZhx%|? z?r6pTH08GsD8CK%v}>KR|2);cbU${RvO}-3LtQ_AQhvY|r2ngwpQ%)SCRydb6N(}} z-&KBcy}}Q~$o43L{LDevFZKKDm0cm4c8vs1a%HP>$zJJdReT;*d~TGmSbU)@_mDbR z;8OT_g$MWh1&W{gpz&d?s`q?V?^czMu-g}uRg~MSYaCTIcDt}o&zog0EUGG6P*Pn} zRFyX?qqL&DD9=$)S|r0lhuEFV9QOGo<&M&l8xir^*)wu$Qk*r*D~qN(Gba{S*q4?R z7L{i@DJZqHq`>JYE2?rNPDrms*AeWN@%@uwN6RU5*3za*Omp@ z#xF5?ncM9Dz54&J3QkOh`WF<{z!#Mk!4YNVz@wy+LxG{suYgLgg*FmXsTGtt78Tj6 ziyc*H2{n!dDfV1@O-W{LW{RCiM9s@Jh$^aUs+?t&R6#+0Co!c;x}1!PvP$A0Gn+Lm zzdu4s)Cm<-Tvf5uUf?LS&tFpRtSNy@bvjB*HCOdNuQh4A@RwB;%cbo?{t5P}I%ma> z^1@R1+NlLO2uQZigs-=kK+BGrimK^c&_r(O=?*l7%uFag(LO%6WC6VWbaKy$-0%|< z&}f_$<>f`rnr_ZKGqW~3&ps=6ihXKcf;~6Yo|!qfG&jYbY0uPnE_0MS7EooQuI*{% z%k71Z8V72Ah8=uPw9k^k_KHek6*{K6oz5LEXX|HkHC}R+q;VBmGr9Iq8X@mrzEo@H z|KC@1DjJsRx3o6p*E8yY8UpK{dF7{7EvW9^izOz=tf-Ge3xRr4gL*#AXei&H2G+eF zA1^!p2~}wEWNE?Oe1bGN%8(mbW^T5`%;-G&Hy ztM49?lrXglqn7j~^XC^;rInUeIH@J6c;vT=s>(1pswsj@_65t9*5yC)Y}EhPe`vSSx{V4UQ}HjRJuRTM2!mv#(7-}4@5{S z$}Fd`s8NK(1bBtfa<}{lsy4 zmYM+>@|dJY%PT1}<}e}GQKO1A%n9l*tyoe;HEPIJYTD9*Gb=4M7ky`?zhZ(-E?Mgd zmlWw^aNa-YO(KXx9Y!a*1TA@XZh}2Ehki~ibu0*qtD1dc5=QvE5H{*ZqoMf)`crCH zMfRd)&Z0`?|7jF3t_s6o=+a(PRaH@yk(il1Lz`)I3sNmIZ+f>7RfN`!tKajhin(jr%IpR zB~p^3MtT=i!gxN5DS&66e&%_IHcBw+Ab!EXdZGhkwOZH|&EX1Asd-oZ&qr%osmx_} z8DWH;JO(wPE-AGBF-ez8>zJYF1T{X!OmVtlH=RiuK!X}zi@_U4f|PDac?lXO%`o}Q zQg7feu*BrTqWO*`rTS2PdM*v1XjX}FT_%Q|v+S@FYPuM9R2Nm%Ous@4$*kow&IxvV zc|}#3qm+$95iNHD@|aRNC(ka*iprdo%fTQjC_x`^mll;Ts4151UN!n4JIzQU9=nJi z7~!ZknVDHqQbM8!LPGcGa4o@^<=Xw3bj?A*6CBRPOG>JgJD3qDL4_wKR94Y=wx*=0 zI=H3tuoUX1O4CzldY(x2pIeiR>3@y(3+>2WA={&_WG5I+bi%RQYpNXRw6G3C!|KFw zXk{f%J@)iGI|kyJx#R8GGtdG`NrYUC7BV$;Hyx%r zl@p>_8jV3ic%5#gq)JY@usgA)0w~4~x}JmlS!k=6sCP_S>EmU-!R3WcI-x)tGSW~E zhQv~KlQD6e6V0f`-wOwgv-Ad;>L@If!%*49=y_^-No1o`&5(KH&b6mZEh=}ST0%5w zYKC1^KXI<+7^Y4^A5Wt}GMK!pSl#@R#l4_1U@h7#lU&*a)G^H|WexM0355o0JF&}9 zKAEPGn&DC(P4$~sp*5!*^ujf8JzAqmOp;AQ#+V+W0l{F=in=iwXuwH{*(J-0O4F*U z9Lv+FXDLcUrOBZgvv7=soHdT}1*Juy4UR9KCXRyx)mBf@vP)`$n_pQejl)y(svPCj z^Wi`pwH)i|) zWOReN`X8@X>lX5J$CtV>{-Si;aJWh64$9a7{4u&1AI~H7@{4A$U26U(rs-17FBms> zFZh>clG~*}XRC_;)8#viZ;Ron3+;=FmfK}V3sab#m^h`(f$mgW(!ogb7xotz-_cTK zPDK;v)6{0UybrB>nNe;&`_OQ<{%q$Zn70wzt^z(OysWlbY6Qr(E z>)B~mFwu_1_!>t^DOO_b2&TbhHAZyy60DFPoRb!MS<(FBdpf2x;bm$poH;&RAdlTt_<7t>2H zSSi8$YdnSzl^D09U&W4|W684VSLDnQjd zLvZ`GiHZ8I7~0xG8s9A_&Cn(gSnLBNV1`*(N|siLNf3r!HHafabFrxCETSaX{VFLf zQfag#@^?usLXm-_rejNPX2k+PvT3RKU?WLm10!T=YG8wgr#dHr2AEb;<8T)HOPN_x zz9Am zB9qF%b`I%lN|j@&#ulk-Dkd}4#e{0USjkEuD&HhDF>RKhs-S=}(^@imoN12g;u_iq zfSapHu5^@CO`lP+P+w5!(pO=d5hXZl60yRHJ`BDQYfLp2@Qp}WjC}BGHV#10_F_e$ zRD+#x>iQ@EW9}(xflJd-p%$tKWmnO$>yR{rINDBpc@g&3b!M^0qm?a9QZs(7m!!so5EMMU4BNOUSszuw$zx`>q#TQY zi-JH6f}=>)lsmPm0xQa;6;;)hILiWO8kleb{rsmoa7Itp8N_&t;1eXwZL9kTv1a|URzR4+xk_G>dF#qnHR`KrYmMo zx0h6dta4N>zq+Do5g7XEDYeZ2nGqQ`mMyfyPdQ7l)={PE{%0^k+XY6s>H7sgSt>Q; zG0IO<*-y?4;~D?puli@`2P;czo9<^NN?TG>@rx1$HYtBzc}oju6Yl4w(e`71cCwrb zG+sLU@N;ypwf*Y6ii`?v2|dWr5|dFq6f{TMRjbY{DER5QFDR1TY=)zf`a;^%{8@?7 zbex*eQ%bZ49LK?224huZOq+T?Ju}BrEOgHuc5I%c{It@&q}ApBSo@r{ppH8 zZDv(2=`xxL_AQAR-(s3jtqY-<>6<&`7GZR07kdRBKZmwo0q-Y?KrRb^~Y=s+dMlK`jV|f-R((7{UQS+Gj4fTY+8Z*4s%6?-4H+&QYpQ==KVTIhqwBr3L2l~oC5D0HF`LEO0#@^A zlBws~wU^aVWER3%u%beQYU4~X+SZwN^e06clHb8<*)dB`JGVNDP*GGL=x}qUD+^6KItD$HsRzInYqphH_ z45lgB#o9at&hHn@Q&NoPDJjPD6fG#kz#iozyPrQ#DJyemCuReEB<-G;Eh$$xI#Rg= z74o==zUj*k2B2-?5N~B>nSTC2HkRatn6S!<)ei86a6PD4s`P;8(130&KT|9yC{ydY zSj4O?!QzIT=T5;1?^4HdqG;EuM83cPDo;PMrzMqXkQ7Tw6PN%)vpQF={%efRH_<0jq85#C*V-rNy z%<1VF_N1|i6GXj>?TKT@jh!g*dGin_d2C{8SF&qpDo+>`Gm(XL$s;MT3z?YQ zg-lA~qLvOiWe>!#D9&}Q^nz@}W!I}0N{T>i{lY7h+9jotzXPE}OMYbtabx=b|5M5! zOpMHg#yU-6`(Z&@`4;eAA_qL*vwdyBn_c4+Z?iGL~joxd!C;YElq z6unpr?ft>gOl zunz~}E$d{uqd~a1S>hc*c!R=)dhbrp->dM5Al$lM<`Wr&?^U=p2-iR4F**p(kC87% zCIsO%3ZEE+*PkonPYc3h)Q3W{gYfMtpLs#}%L>mA!W+iR{5#b9kh&f!)eFWEsvI3} zQMfe-k13M*=-;2!DwkeMoeEXrHCye{znc(@>+kaiq4V3_r>8pT_W0 z44=pFPZ*xh@I4H#WVrhDhZgE$csq-~p5f?X{8v50|LsSG*vxSHPP%?IF#L!P;<=6C zBUpUbgK}hGfz9{K2Mv;n@Em5UC~e>E^X%8x>y49BMh{MSx~<5LsBr0P$t?b1hO18#XrV_L{xyr=!SD|mE|@)B!SD!%w=g`C;h!@+hT*vk zw=$f*-LGGx8GfA(;+eqk>lr?g;ZBCz7(R*RKaJt#EPgh_7c+bw!ECGJLc8Za%f^yBRJTq@OXvj|Jvs82K`M zAB!K!a5a?GLSqG8d?Lf??@;I${&a};lla%?sRT@8 zxc+x32+wACZvxackKveD_^*71ANHd{6f<1^h6e>#G8~_5@n6druFo?m(8X|lj7Io+ zhM!G2+(;9iE~(<%Pz0KBi`%O`>1qZmGs;omUa#_;nQK8@iQFg%;# zqZvMr;bRz{&v1O&$A1+w{6aqp`)Bw?3}43Ziy7`>_*jOoXLu{a>luCt!#6WLf#D4d zPh|Kuh9@z6JHyq~T?^gGaQz!R6wtzO^+_Bp{vL+=G(`Thm*FW4Kfv(u4EHj80>ck8 zd?Le-GCY;x9SonuaM2jB|4SJj!SG)(Jd)v;F+7IhmowbTa6axD&2SrwpTO`mhEHU8 zI>T)Y&tUj8hEHL5HpA)fA?epVhG*&^p7{*_mf^(=&tiBb!>2KP8N;VD+{N&{3}4Ui z84Ryy_)LaxX83M~H!z(3rjvecWB4o`#B)2tXES^!!{;!(h2hx@-^1{KF?=t>k2CxL z!>?qxm*KB5{4m3F7=D!Dc?|DhIDM;a4-<%J6v% zAIua0kOH8U7~2 zmodD6;Vy=&PwHu*>lt3i;@300h~b+VKA+(Y4A;NeM#0+{zJSHw&T#cP=i;YS%>$?y(_S20{X6tMqlhDR{GhT)M6 zU&8PhhSxIO%J8KOAIByq@7bm_Ogl@S9lt28PG8_}du1j>X^3@S7RF zli}+b-oo%(7`}($hZw$>;a0}a0fyho;(Hmsf#HW4ejCG&GW>T8?_l`t3>WmFXIwOY zJjU<{hX0=7kqmb;Jci)|m|Rwd-^t>SX81;iCoudjhEHVpM3%pe;dit6(-?k$;n@tQ zwE+E^$M8)$h-W^-H#5AL;eTLwCBt80_%epy!*CbF?`8OUhTq5VdWQdl;hPz*KB=mO zHZc5t7JnPVA7J=)h96}3PKH0o@D_$QFnkZgw=jG!!?!a00K*?*xR>D%GyE{aA7S`W zhHqnd2g5xK7iwLI?7xZO5e)wm!y_5~D8pkI{uslp41b*AqZ$4L!xI?(B*P~%{3(Xp z82&WFr!jmOv-50*t4|qgspc{K8J15z!-+!^C-e2O=9oaKVNQx|PunfNp1WBWQDvK=}`Yj$?Ea=yXOu z2D*UJPk^pt^wXdl8T~BiRz|-Fx}DLjpv~cd@?QZR$LQBVr!)F3&;^X%2fB{Y`$0D{ z`a{sIjQ%(1c1E{@Hunsa{|V?gMt=@EozedRUBKvXK-V$)Br zj|E-8=p@i}j2;iVkhD1R#GI7VLqI-Sw8K^HJO2Xq~yuL9l3 z=xaf@GWrJ4?Tjt}ZSE5&e?I6qMlS@N&ggQ`1&po&UB~EJ(2b0~5p*k~SAuS5bRB4O z-$426K*urqR?z8;z8!P{qwfG+$LPC3H!}Jj(5;Mq0CYQ}8$p}T(#zj__QRm#7~KRq zozahhE@1Q%pz9d@H0VY~KMT5*(Jz8-XLKuQbH70OuYitY^y{G08T}UM0!HrxUB~GC zpc@(eA?Q{{{~L5WquW87`v=PZ1ausuKL?%8=>LE&VDvYj>ll3!bR(m`1>MT%A3(P= z+MJH^BLn4!gN|c#Z_w$C?gzSn(E~u&F?tZ_Mn(?--OA|mK({kG4z&5~K>6o`j$?E@ z=yXPp1zo`CB+zw?9uK;a(UU;8GWv4R?Tk(bZ5|LPe=6uWMqdFsozb&F7ce>pbRDCw z0^P{yYeBa%`UcSLj4l9ejtZ1NA9Ng}7lKY_bUEk(MpuEZV{|R(Mn>NVx|Pu@LANuy z4zxKsQ2sj5ag4qdbULGN2VKDEJ3!Yl`fkvTjJ^kSE2AF(-OlJn(B^@G@*f5r$LJ=| z>5P61bOEEE0A0uEr$IL|`dQGejD8VxJEL1cn+FBTe+6_LqhAM|&gi#57chDs=sHI4 z2i?f%4?(vw`rn}28Ql)rJUCGPC!pgP{W<7#M*jzN0i(YGUB~E?pc@(eE$CK8{{Xt3 z(dG=49}_4)9CRF`dxK7AbU)Aqj2-~Gj?sfaH!^w%=vGFb2fCfnaiGoT1j;`jbR47O zL8mi%Ea(D8CxNbG^mx#XjGhF#mC=`jZfA5lX!8)Q{L?F5)aFyi`UoG+Z|tou80xN$ z6K=01swuUzb5k3hI6t=iSRb1Y^TNHSSF~V=hAJ$NWh8 z*&{1^2j zyjeb*5Ig2|b`B;?mjOE1D{MpElqMF>$wC}y5h4%t_Sn5yJ_phqwV0X&=E*9rF7ac9 zcq8unRqv$rUA-ap`?NcEAwKlDJ^fIY5Bn8JYxbuNC2xNEb*VO!NTIqAm03O)(r7$Q z%kW)%x^vS3=-Vn>Ir-^BQ_04to;d&iggm+|^o+~4HQOV^Vw+jySs+uGDeK6Auh7{j=?rh4Hdpo;I;EY;S(0uz9+}peD{NY@UR`{>9%9dd3!tJJD<<;Rh!N3 zwQI9{bNdRW;BK^S&Z9c4b?I4`&uYhy`{u=5s zQn#2bZm$r@BOt?Mmq^9>_zo}X${*(jo2E~bu=!%cY(6WcYfVqxLUVuU;K@3(dqk#g>3~V}vwt3>Zbm*4q{grL+eTO!Qwn%ABr)Ao& zQU4BtvXP+ZQd1MxdYh~^xm(aMCpVy7&Vc(+4# z+u0)TF{HO5{rQL=DvwW)N17@Zen`r_89wmkmdi-b-af9&?pHih`W`0NzA50HWLk|o ziSztV$+O#lXQ~_Gc@)x6{fJ0$CAE#iy+j__@aYx1HJ?TNcS2XIRJoya70t`tD9f!@ z@tuub>xpy~L1lh!d%<6~kWqw%^&=U@L)QKzr<2lmbUyVaY}p?#U*`R5gjv7t+~G8b zb*TrE>1%}5sWka3ZKpNcxh>1*jS{CR|5=ppkxqXd>iVO!?Fgg3MlX-#j(td$IT>NL z!<{=q$^S0GE@kOrXUa55z#C37Zed_B=0)9)WT$5Vei z9={#;8jm(gfS2 zKAw1_F$wiM)CbkYg{9^qjms=j?!0P>XO(&2kp}qRy9-QiN-zCgbW=R>13%e<`#CLs z#{}}FFDXB)`&!*rNroPXmxd*CO@@Zc+#h_a$xsg&>WN2PhA8k#@qGyQ$A`>+1AQF& zUX01pBl0Xy2Kv7iiyi&8^tXowr+E$zw)xH3V(nV!QJm^|Ub`g+15&hlt|Y=-FK-*sJ>z>5H1`RFtCz83xZ4%%zb z`&+6T>J#)m2jYd~5x=&qQ>=;Y-C+)(Et3jAyJ7qWM$?#sSc2n`LlQG4y-9!sbf` ze>lHV--5odVWN1-)m!$3t5v`4N`_Al6LV_NZ#!V8v97*uXRFCwh%rS&Z<{B7fa|gc zdkbIwtvx)4`bT);`r14NxA(&{&9kc2oVw};;agQ_NiDd&zh@O}u)*xQ>|-D5)+SP@ zJ%ob~vXj{CEZ->j3e);D=&DD&C8G5nr^|e=b(ymRcDiFA#wIAQ8GD|KyhBCaQ?Sny zkd?4Q77@Q1@nh@F_pTDABVLRbTA<7Pg`&B0wT@M5%nh!h8?)LtDJO>f~7R2>lC&yfEPK>!Q zHXcpEj{LiyKlzp5~_zelwLfsbhkk zKhcLF&n)7x&kPUg?>z8*lKhO)T?=$K7`hug24gztuHjsf+@|SnP}8MIe*@Bz>}?i{ zdpPO}{xeV0L6~rVN?}TtQpDB$!Ng${Kc?v*@iU=gLO#Z;ZHU)7NBA7DFXXX9lS7SN zDK6RhQsg(d(B?V0#_YZker~d5@DbQr-WdGZTW>DLb!$b(>TuyZ1^-52`52?xEH>X^ z)EoKH!6>gCc6JalQ+&#|^E%=C68U}y8NNfgUdvF5J76uTkl%O6=icu-wRFQ#A6j~hb%}>ZkdFAkcrkAYY(a!0FU$e%0**eR9=@br z>SSP3`fsv)G)G9c33uE~;h7BIH#tT$kH9>4HR6*VgZ1LwosnvVZ}KjJENd)oY8(DK zsHU;MaIZsJ-N&DDX291>@qAaEiLuFEZEVtzF5Di-`cz!n&JNUJ2lPSpKU@s_lZdf> z>pDl|y>z+wde0y{!^PLDqeb2>GujYzhdNI^VHdvP#VHfLDBrtC_{bK9i{RVAfxH#uXOM7P?`SrN|HV?HK@&)bv zdwZ?`pVW4|mSIhEAkQh*cD!giF=#t6{o%t8+EVj-h-6cL@;if@N+IX(k(Rd|Zx5-n zRP;wwcY~?kCy+kRIf5~5-U-b8soYfHt&mfI?=+JuM}X(_81$F0&-@QM^C*rDeUfRI z81Sl=W^mI*NJF+CDosNhr5Wr`(+l-M^*bDC1lkXkVMhMm$+8TA`4olgWzhJN_$0oG z*S3?Ld9S0~G}N~h_bB9J!gB(J7lP#uciCH{lWQT1>z-!N$9&gWn9y|D4jdm%}zDq9_8-IoL4dKH5 zxKVtyQhFAA4qMF z%FxG@D@m7#Ux;uA#wX;9Vxj+uUE&Y&$FD+s-G`pQIQn(O`!mYabdS1x1@woYiGRY# zC&dc$8|lZ$XPcWY2j80zeo}>>)cOFezZhUa-qe;>qYp?%yp!y{|(Yl6^0pFXH~4qMdlQRbU+pVbC??GBw=}eyHxrUr?G2h(mr4 zVeu_$Y)dr%O|&K2=Iep}x)5uD=17d~MA!zW5aYKN7j_gB7j)?BffP4X`r!PiP6hfx z8p~kL>C@LD_4Fp#$lp68Z-kl9x4yyYFxS3U8BKFN(kqSE(_hZ=Y3tg^e=_<77y1nf zvx5J$2J-h9bHcvO!j@{^CKjVj<`qMi!-ZH}ZGP|`(hpygjDmdxV1Gx6FB*TY9ICGj zU{wA{)b}b8cEo|T!*tD$yK-V-59B9W(AQdpxd~~%_FAlTWNv^E4-N^g?=X}>;~|>& zI9sxOYmw)J!g9m`-TzeEI%)~i)|^QOKDUoXzNB+n%gncQU(0ksCazzH)+cpcS4>79 z6TGhIfNXkPJ$)?tCf!yYs;_zbLb34^=vQA~{2S<_LHa(u;yM5PP@CsNkNO$>#wir$>e*eF zuqREI+WIZVo2OU&RU2p4g{OPgqTd+-|KJjq4G#E*)y2+^IQRydQ<472M^HLSL;DW8 zZuR*LjSWs>Y!F-zt1&jx<5E6U2b2%h1C4q0{8nrAiZxfwMknvr-*5ds?H%#OZ{8t3^A{fcm22VF zUpZlK!_j_eKVd}2syiU#c*l*-j@37g=vX#FxHp?)HoRoUxEgIQ&NA=~IiDEV6k{=O zc;Q6nCY)tedjn{PC_4{2(dI7L7dc)aJSTe#?7yge0?89YZ&R z#Gnl^R^fh4s~_4+iE1J~&1-EPahNY+o^xaic>D*#HJ#Ppeb1nyzkTN`&^BsI4JLQd z$<9sWPYyzlX4J3d$NQjs*req=0ZV zx`xHmzE!wQmYv@m8Z3MVCy3_Tkhcr`ddJzCPp^1c(;e|j{UY&4yb{l``DeL5Kz)Sb zH!6tVMT*~ZHGX$`Xw8=Qd1S9t7ic5<$Yz(Oo7_vV_C)r)+-!1_{jO*iZbxr1#ss@u z5svzE%B%Z#eerRt>@0RJ?d$IQHFE#o>+@zE123cOrb%B-kx}zl7hhNcSB0 zN$!a;8)T_PTw!zV+ZZn1-uOe>J0JFo-tZ*CMc;_oCK2IoTWIn))6MQv(09W+;VGyK zb31T$@>%4y1p6v2Hzs#{yT;_sALP31uw_V-mC9e)%d-l5(W@{=U$wHYXVpHePyO&a zPbX}1)yn>!msTQd0fu2=QU|#_0fIU$c_T|iw&xAV9hi^@X-M)glqTj<;nLH+ht+L2II~HMU zwEBr|BKs*Qm%fMg-)KBsp!pzk(~XFekNJQ&-MJ$L{%JhcQlCQme+m10d*BaZktc;y zeifuY@bPc7m0zRW5vt8);z|9<)xdasC*o6`O-9|ssJN5xq&)tHJZMdj#&y?dd=6}K zb#^|r2YAamTZajCmkXTAlJ?%*rN1P5ih6gJwjWDt;pA5+Uj9_u&NT2r{*2oB-H>H9 z);h^guR_@;o;11NfevWDlj0XZchnzIcster_8-R{Ci;zB@UnK$91q69?z(>2oM&0v$=ff9dlzo25l(H`-WR}tXvx(^^IE?f(5d5EB@qEA@6~8pzJQ0O8g~_6M z349CL2bD7yyflE9f^-^xo7^U>t)xeaxAnP5`(#^l+!V3X6eE1ZZ;eX#5$TuuyeO(S z>9gC83HO)BFgHcJIEHYFPj#TD*$0etyA!r@dd1Uz9@{PuJ}3A_-1rlC-Wem3+eetN z#vK(;bsh_Q`Ulc9B0c;~JjQw{6gB{H?^jqp_G<`hcbeR>mF}EZ5g%(N&E9@khX5{p z*6W(jqIx@FF}qJ@&^r1+tsgZ-VoVD^dU92Sr!&KxdI|B0wGYA%S)$|Nlj@xoS5CUC zx7&qsVZTS)`b_hPN1+qRyMLWJA8T#V%iz-@#SRzNsjb+jUahTDM>V;jpB>1X>~uDs z)V|4w4hFvg`;7MY!`B19QCH-r84`5Ds0;2w;d z+6JYkv0e-I2lTYhql`H0BT#q;9}3-O8L@!nC7!2hZW%%dx4K=DZ`~whIwH;Bq(XfN^9 z-W~*wkDR5TelTB*imw264>0l%-f-y)UI%uU!dwHDe}G>}gS|Wl+&csNmjgsf`?!D| z5x+YSb~3|+xhZT2HWjd;9-huMrqt00#~zWip$nxAQGJ|%y-{7w!|!gkli%On8+xGj zPC6e7-n`Ir8|=+*JE$MBonIkdpl$=Ub1l+P87GmZyWi^R>9m+rO(>i4<$h~-xb#~& zR1ccp%F_JSRM1p+0%IN8o7DYQO_cNlw%!3h;QXd@(>ml$@>%iJ{nlra*S&4{JP@^E zeg1HX;+5*-O#Hrwd^x}A;BSrM_k79k-nKrH-ysJ4?l`Nv-^9tf|KerxYf^IHSs|4BNB9e$`iWnUxL4qnsx8_M@9gc;>4PIUEc51}3XSUuky zLT{FyvYrnRAE*yqZ*PJQRnO63-Szg_*PWaCB0kmg!%S}#26}r6ILY_S8RQG)@83uK z{kY%5O}yb1R?Xj0Uf(O($S-O>Rke#+#3g^U4P{cAQ_xepYC}YmPir6_jQF~*D$&vm zZu*q+L|Y{v=SBb127gCw&4KVE%7-0PPdYC``O%Z+;`0n z&7OiE6I+LB^GB>}#QQ83?F@zK`;Q_VLQL2Ip7YPg8B@d&SdY;0n}EAUD%>S;Q>+-T z#l!kzL$qnfan$7s8naSmVSVy94_xzrk1z z`;pDD7|Ut+XsqSp?0^e%YRV^HW|IFoE+;%BehcX)_xOl$9TIft-c>6*PRCkDL6 z|LOH_+Q-F=`2uv?*z_^+Q$P7;I({#}?Ts2XM)X{l^Tl_f`Aekx0{djGz`yK2c+6Hj zKVKkxz2~gU`E10HF=OzYhWpc@L&q$@a~Fl6issKPrsmK3n3@kqnVLTxYHI#ugsItkA?_jYzdeE7oiXOq{-);e{x;vogH6r( zx7vK)4mUOXE-*E>V0`f{>hUP*7<;Pm!~5D6r}Y|JEg*0lMse^ zD$YtYQ92s)993c7%Mpe-Erk)^6!ur7?NGd~9*eUmQ&7j4)5V}Ju`U&V4#K@Ey>%M( zX)*EFz&>KPM!8#oLvK07i1R*vL-u&vOp$`U^2N4X)c@xCoR5L;#Z7*>WnMtmgOKxe zg?n!hDUMd0@fhIF=>_a%g^MDQf-%V6SXVUqpP2Z!kcN1@2;tAEI4uh$Pd!k7Vc-Sz zn$zm4%SprYY4G*9isxMpzWRy9-j%S|TWy|}m9{&B&*f0RWQwpge`Gc-vX#lb2rulG zuu;FU-T4sC<*eOk#(sL3b}q*T-b6#Rdl6OX=Uuk%9qQ0oBqaIA}*>1;oxYs(dAbLQ0FP(Dc8wqd%~uU3L4{}J4; z9)CpoTde#0$Jf6*+PP^vbVC023Ox0GDhe{RQ=YiVC+hv`HNcnQ79VvcYw~&{-Nh)6 z&g+=aUajyUanZu}d*t<%wkI9kl#OtszQ>F6Ae6s87AJqFtv*P*+-*V(j zd*xaj5$T?eI5d7Ee`N|2zQqWqeSWTgjIncQJfiFU0Avo*`%%lqvhNy${J1X91szhC zZ%bXaL*eo0QMmCm^30`HoWZy~N~O zZy+DF&ok9SZ3sShD?VQh;*;8UNIv&SKGC=MZTi4hIE#e5sUGfPeBNNd=R?FN>W6%b z8T*2)719(Kjkmw1G*N!vq@5jLem&T)w1V$|?lgaXG(-87 zD3rtX(GzqieH?#O>ciGEppTKrH%sZG4Zn52(u6!h>4W-rUf02VZVtic8pY>lWUs8= z{so%WhqS&BbF|fF(}r77gP0bBzQ^9(^Jd~a0oCNx#3t7%^7JG2=PFXk1D- zjUi)g(QcA|HT*WtXEy%^dtIxwxyi7m$H2b}W8D1kxn-F1#1}kaJ1uMl=f&bq$9*H| zCc%}{f@l7^s}N^Me8G!d<19v)HPMyx44w}2)Kr{LZKnP?9~jvu^%r@M&^fAMP4VDk z0^~kZ+x38_fRBmR{PRKNMeD-fB0X$syY6%U13HvV9lur970xsG`^-MbcP!$QP3^&N z-N(4WBkF2RLdZ5ENy>$}VL z?w6PkA#ak+&1AdQKsIgs1lbzTAX|RWdhlOIWFOWwln@oairoLEH-R91@5b?+3rg^4!IQEzN*~ZY^^l{{w035axpHJ%^ zVtvjwU=*J)SE@*9rTGGG7w+i@8>zy)lSPWwp7pp>Y5f=L*NEo?PeXKkduQ^Q+VD=~ z8EC^%S{vS)BK6fPNMEOsXDC~^vqI^sJfN>3$oDeDCp&*1zx6iUV4$y0s4Vo8hmod0 zj)A)QSnn5I$3S#W@=UU=?joD?PZ`P<_Msf!hF>v|Eq9@m&5JQTjjzei9Q>klQ@E0? zfyq{4AlqMnJ6@Nza2M+55u~w=$9W{+x-AIc7XxnqJ_oq4uFn}o{DYnWTDJqNb;rMo z@@*4bIhmC2KAGp@2O<4=f+p`3j@87Q(50ke&7gz5>6=)@|p>^&0BmV!hFL zcOTy-K-}vPjy-{R>m*kW^>?myw&$@A5bp@Ly&w+6-VlrXZpt6wlpmLUJ!EeI9uE9* z+_p=xu7YPB_R?wH1bN_$cHo_+mZkFC^GI#&By5ojW3`c(LxlmqDZ+(yBvaECdZ$TS zJ24$etHpY#B}`lIBzv3!c^%3g={$d2j67?fHbUgl7{I5kD?~L-GKh!$TrFPJkJ##d z$Q5XZsxMWxdI9q0wmKAasCIblN@=TBybGk;>O|yA`lfbx0>AZk_#*NM<+sA1%QLm9 zN4xMT{Y6Hy^cSa54(Ic0X$StbAHDC0nxXhSKfq@{Fqn2q3pd_{WS zcZbB{wu!j&VsTUdoIlx>L;X>pT}8(~aRg-mr~c?4xUDw$BRr4e9se`g!)TNnY`44N zA7~7?9DH+oC<7hJZmTCNA3G>u54VD!KO=9l+bleFyNv`-q3rf5;Ai6V2jmyvQ`+qs zWw&#J_{=ci^H_@FlZ|=uk?$JC=edl}Pmo6_J}&~F)E>#VkiQS>|!Zr1I|9^e&l@QJ6*|n5Wn^McocbrlJgrX zD~R_*er)#Zi|GG?eT%RiAfmy!HINEt^+8F7ZzFXNY=cW#PQ zGBz<8s|{pqRx&mVDOhbKP+U zeBNo1d6iaMgzD`M6|PPiZ5c!A8=E zFT{;9h`Sp5!(^X35O=(a%YDsbz|T~7o)9`2fj)$E@-1Y6jrn!*AJCy}Y#jF8Y5kh} znm)*PtkTII{MLPq8$5>6$@?LA_keej{axe}N>94%>AHYrR z@jl$ae7L|jt)Gycwt_`BIjk=bMtk-YF4hS5 zt&}h7^Vcdpy;ntHAFHQ+-l0v=AF1cR)RXoC-^aZl_j|bC#r+O$dS8U{q2GVUt)KJU z@vVIKd>+cBICNf`d}cIgY6o7kt(o@S4`a^mglyE#Y}8&rYv=21QEne_vVGDE4ghHW zrw#kK{xecaH^)deyaU`ef`w&Cy?ueOP;I8S(!(FYS3B+BB43ije{KYM>*q%PafbZg zr7-Z9qdlXV!c@L`o%oPHJ92Q?ctvO zOxn!fp?kkAWeE2vg}(-z>hwX-X{b9YKh}1(yBc&r=YjV~{;bs}*|~g=q=(WOwZGx& ze066=*xRR9m^`OZ&aqsx2j^Y3*Wnv|l(($~-@CXD?-n`3@P3i0$+xHnz8Mhi(a+C3 zMf~7$oj|dw{p-xGR8f1@8S7<~IGV^q6r*0P~)G-$17P{-$Ia z@6s7W?fl3%oL9hnWC_mLH}uB(fH1u=+$O$`z`JHUF%Ne^H!&8|8#ld zi*w$v3dK{k<-&IW?;Y*W5$>HI3eQ#0<$iqckaSAt4e<>!cO%L@h;r%u z2*M~VHh-8q6=!u$I4$lBeETDH+MaJZ#+lvMto!7fyF+%k&N!t-ZBx5X}!l-bv6Gt?iQ@ z+kP+SakGFYE8LbQQsP?V85&w^sYBSsD$F`whS3=s3R{7&^Ho^GT+9o5+s537I{thE zzNLV(Ms!~0f|~PkZ^C;aGjL|81UgxdXE@Fy?WQwKJ%^4t1V3`H&oEkd+~->=a8w6p ziY`2F44pChY?Z*bIB?3)5j>6WT3iUoi6hJ@wM1dH=NGs3~eI5 zXig$@zOE4|b(hF6{rryausV0+gbv`}NI%n{!(%#L3H)BUe$@`_ONF=mR-_z+&gjfc z9Lh1>=gy&Z)D(mtQgQNc#d;~Q!=Sg^d@_vK&NT^LVO1_#7K}V}`=7`>% zTZ3~l?I1E2Zqi+0YY4reCv8-=@Y2;QNDP7lq%cTP|?eN!fOJKo_(Sp~;_`;H0uw>^&c zP>?^y4nBeRD~AZ2p+I_pvv)4MLsgL8{k;QQgUD-#4EqP*%!zvUpcC)>IItE8>H} zHN)IFCcLvv=biqDJlCsq-Z>)0i*%G{eyeDvd_P9GYY4s{f%KFo@v#vYl}BaeUmG|l zMq#fZtVV^|76i_TQCKU&%2ZfGjYx40u#E|iU6a#p36JlDzAnJIDV(Fy*1{=%Kg4yY z_+A&|?G?$}W8mo;6`sFd%1nBh1}s+kpR#@bY?`7n6Qq zIcOK~R~7D1IQ2K@sWM!DgiLX^F>9f#Ppn<_=j{9P5U#_pvtAzZJY?;!^YhSamdRH_9 zecBYfKbwjE4Da3HeZ6D3>3H`f%-v$K?tnXW{&sC3PzJ@K)fg zl6HP=5ba~sujz-m$WJ*df^=>877m?XdlRzCdCM&A{8|fW>U+sA@N?n%`8Ar$t=9Gh zqMH8o3F;H^$^YB~S@n5~6TG2)b(_n1h-b8?KOs$>Tw|`M@iYH!OsMn1G)@$*f$l#e zoz~Z*@7|B{qA}jq!f+PtVT92+xYg(cz zPU>-Ro^B(LgRw*0MR$eVP3DW1CS!whtP1#lxCk0GyM z9uG!J9;bk(7J6@BgS3x{20R{DXArFHti)X8yG-#ofbsYb`(C7NGXW;Sm z?mX6yPl2?HKGfD_N(A`RIv=_c5QtwHBz zUceZe`l^#7s7*(AYttt?@x8YZmeeaLZ+xFhw&@}9amf2%)K6^FNZ00+ZTfxiChKg5 z)~4S89juRm*weo20kNZj)*_&fEvii){ut+>l|KHc=_6WuN3GmIA3Kyjo<4&5BB-5!Sp>iTiW_Ac_KHdD^(n)<6y z_V`;R+p06j=KZp(Y&7pOYG>zkkxTOh^;?wPrK7V(#tn(3m-AH>L96~pmJ>&cd*>gV51?%>ljC`+Fy7?NvX*@^k5ziuzP<2lI z*_mu}8}S+3?_x|zYk)qK!+qtKpo7~T+2+PIvJS<-fNl0hz85P#U)A`O?}pYJ@JapO zne@4m_+-9>^f_XnvtKspA8rdF8`VKF&b+u_D-&-l%^K zwlA&T7Im+;D#&)1lI;>rHd${S$RkucxLnnn4dF)hW()G`vwx)CJ^&BF^=26=>n+|d zo2<9*22c z?6M!J#|KE(pn5#0>hUX-$@}MnphMMTN`dO1u}`eGKY@HlC|O_7WR>-}&Op{Tfm8c? z3u%n%QOXwZ?c`kv?6rfPo4!Ea)c)3K zvdQ*G#YrWgsQg-RK1NsxKX`fp4DaikOrG1J06BKM*0Xf7CVz2KNU(J!(?5Dqa5Cs ze+#)n>7zPL?&rDKdAY&JcaqY_yZEj9BkC_h>4Vxojg>w|8Y6v3*#i9tjWcOo{Q%Mw z81y}OR~tGEoU^~{N80t`5c=4y^wElPxIUf+9ZDbXq)L6*@ZAfCIzRYMJLcHPo7#0f z)5kRi`gjmHt^d*f&H<#=*8gU9=9#p!If1nieGIA3wbMfIZ#h@i=N&=(lYa@t|FQ9s z{}#OeuJgYYJiMa#&u9FT{f4T~3avh)niiuBhpHRCXWceT?xikKbx8g+R6NIdGM+r& zY%I4WgxqV+lXB+;$vw?L?&^4D$M9LY-2acg_kpYOxDvnT-uv)J1Vll^KhcXAi^dp0 zL`XDVE})p0)GBS$Y<9U9L?xmSKx1N3uf|PiWxG;QiGMM%tx~r?+LA`Mwq6svYpdPb zXrgItb5Yyc)oyKL5@<;7?|Ys7{@j$4vQsd4PEBW_`-~oJP6#gFCd`O}zJ8?qNo`1AGHx z)z3?mTl9zMo$8Qsi@n&N{H{04EoHODy7i2YzfGRPkEaJHrsOT5tZH%PzJ zOdksWSKzneNo+5kBDF$CCPZsgsA5UsH?_2ho1Y&x4QUy=8m^`^NgeRh( z`{T(>cycQ`uYj+oZ5ebrOgg9cp>xtebaM8P(COmaXshn#fVbSF(-A`F=>g~*X4YK- z;r+?1H`1u1C(SyF8X(?!bB~bhhq#kCed|-s+5SvF@HXl*88~Cv?c@3hze)JWe&9*q ziw8c8XDCk`&k&wio){j<|1HYjZ<$zk7Mc1gIPAo=@hF}k@jvAGfTxS+eV)JZbnqn)|p?tL#NjnjpnpC# z-=)#q_u=DAlZVapQ%K(p=UkH_6FxB%{)%dN(1c6->yE#{)=+4|N11fU-W$pDK3vW? z9Pu#Emw2Sk{ut2Z?~H^eb8g^|c=r${@AsPSOk9g@*>59ZGjI!e=9=%*aPxWccq&a; zDX!#k#>k^v#`p7XrMsOtSv;TP$>b5-PTX{!zvq$lZ#mx&*`CaKx{q0Bnoirl{$Z6X zdu2EW%@Nrm=UB~nYTTlA6V+pr`2J(v*KCgs;_Q=kdxkvrQ(RE?j31U;b0V zmrdIA+8@qte(hx3xBfbw^{bQrdf~61QO$on@K3-0YXoQW%&xaR_EXOAUAJq zTz&3$(CqFiJDYnta`fBem%L`&qKVu;w4ucw+uqJzaVM(^6v1
5nZ58Z&H>Q64cNi z;C~@W4Q<2!eesXM&wDHWcc3YOdQmp`$lWLH%*QvAhacVv&Ob<;C^amp0~xwxG5A!l7Cu!bt~XPiH1YYFE(Q8^!1 z?%z-mxd*q$a*q#tJ-BrBlv3fR+$!Q!N-%HBVom*WW8jIi8zMJsaz*+!jZd&X)a7 zcFy7OeUMyDhn}XawA@Uda z>(gh?ljpnk#8d3`X(=JEH#mpa%bBQd@=J_P+%#Kp7C&}?oBdrPFDcZoC){;V&MQ4f z+9&@(9XxLzxk;~2&X7%HFN=aU#aPAv=o1}2%CH?81xDFB{|$bBfqNz2W3SF7_$B#m z)ALEZ_EYkiE&Z#0E@EO!BzZ}{D6jza>`Z|NoQsQmZL$$|i7+R9r1Ef9@LQ~}@>q3g z)TePix7^{?Mft4z+O~j0>UV_JbDp~|BDX{8G)LJt)!0XDde@<~{LSHLdCxxb)Lzc( zr7ntmtaX1svg74-@b8b%syf!475@hO{)mp3MMflFPq_MSK6|l!@P~7OT6T);Gj0=k z4Nxc2E@U6o)!%YgltcaH#3Z4E$EQiQbhP-93mM087H!*UT%pHDUt-ZO^o%oceKREOar~F;NvGuO@V~*U_u&tBY@HGw zc_>c_XD>@y!861xr<1M`mr1)9Dh?NYfq zy~xJ6Wy#r=Oju|Airi+Y;pqM0r^MEH3)wwD9SXnYUI?3(+cBFvI{cJdWOpy&SHbI4 zMqBsmZBg#98!`DPbcuY{>Um{$=Vjcvd%}=C~p`xWv=FNbb?>LI~$y*@4r~iyXZ;bX|Fw1d>Sd#YBOG)daM zW?AG80(eo>o??uNMiNK(em{8#{qMok(DSh0Hg%ia>!8~X*r1}%q+gYGaGbKNLAMC~ zqFa7|zL9$n-oh4KhHiL|_8@ztM^c|sXCg<-uoXm3+qfTPH8K%=G%LG|zW2ppu7i!( zPU9Td6Udq+S9SQg-@2`tu)Vak0Q&M0{TO$`wLC!nWu|N$Tb7wEWwB(r`wlwVl%?YW z^yz!0ZK1C&!58U|W`aZLkT%*MEyv{yHE0(Z|EYeSnxS+40p3a8qEoWyKi^ud{vx!# z!5LQpo*%&TwM)1=gYu7$Pz@u)R2%!(UK-~Zea%HY(bHC+_cC#xfOpdF6#LE|M}MzI zf9vgvxZ9vx^sS5gFoZ9{$3dKt!P(U{Hb;o4ThwMcC7{ zvk$*6`sXC?S=emSejf&(>1tz9S0aU?uTE;;H8Fs{uX^^I6RWP&4w)~|=P@pF zMl|&M;9qkhcP5dSw9i5CO6s{dglBU8K<E28p>-Ene`$oCWHU~G zX{`M8auLrOOP?eEE#JxOj3h^r}te~@a#-7G;^kQo16Ym z?no&%$LCJxkOr~S7J^68S+pqpQf{H$M%wkrfP{~C*^lV_4+$5X5{G=+1h2%q1kUkQ zgK|RSN|`Kt`-m&&B}rUoq5Vgp@uUq&{#IR{g$6HWJ`Vj?KVx4ph;*rXTI#|zq``T7 z{8$@L7fq*XR2vLkV#<1Ua`RPvG%7d@ArY%wN6xsvVfvjfpLjfC)Zt z3H{0@?x^LAm3Gd>RT}%)Q*v{#o2UCqZX-VD)iaj;IW!pWi7j$}&`)`9ADw%okvM!~ z(J44LEMY9b{_`IYCiJx7O8r`T%ec4dHQg=>U>7LvV|kDAp69OFzelF8pr6*c9XXum z)$$D;M|{hR;zCdJ`@u|oKGekb!CC{)a?*+3ko=p>e7Bit(nZ7FN_9U|!e z(03bYME^>>*GVgVQUo-9()^A}elOVc{Eqxz%zluzYoQqA@30~#~ zCOq}|YF6m=zcJ^P?} z@F$cb(T_dtX2f#E*r~4^`Rx8#dl`eRf=?d1Z`N?ySzrd^#Ax*rw&;;io-xl*R0Dm& z3!!lzcK)O2R*Odi@zEFEBSY%Hg14*_v2FX}v+CD52%7rT(Ryg;j&Ih_`F?QhGw(R$ zj%!`6*{7k`pOj@CvhC+QSxZ)|HV|XR6o(j zZi*$`Eaz_tKOCBmiQ_~+u;+oT*V{tRos%`Rz3(FfK9k?fsq5_~7rMIRnmBJGy@&K= zv~OLXbKj=OiNHK2Oy;)|Lv;Kf(ectJh@O`|!P50teibUS3Cjc2Q(u|2aL+Sw+ut&Ifaf_~eiM9CkCZFr!<7HX9$6j*6=Q`eh&UH@Cl2F&)`x1C7e zVZ{-gR^K7GrSEv1wy~A@z2n#|(g#UDk<5IlaW~AX2i(8Xu8s?gF-R|Ifogb$U zO!lNUB)izlr&MRx1)=fB2Q9eBf`rL^pb{>4-X>3x2OD=0TX&rPmnjogy5xX??`nVe z2n)scmW7XbISb#fOnfEg{n&oyMdW^L4=(kfkKN_lcA?37*Yoz@rI*8kh4Nd*5n@xv zA98mNZ*_O}y&uvGZ=}BW$=!=@xI0}j=;znmol4yAxb^#A*8jk*-|P52`0R5JZ+M6_ zzVSmEw#j=*Y{N}?|5|S6wvy-u-4NvpRiY4O4KpeABq+5?!;onSMygUCO2{;x+9z53=vugFQx= zk8$&lg$~BP&E{Oa)sAFrvzPIwkFj!?UXKZSTeHFy_3x!Eio96%%S+@b>nI{JfX)%aK5gXeq3XUd<*IV5}QtT9(I^A|FI<2aMp z;O*KQsm>)f*m14!57vH9*LV8btS3xgdpFw32whf~&ur*qUST18lX*0^%$1{4FL91T zc9QGJN#Kfh5WI`Ihjn~Mx-Ob0=HOW1SBTHq9xZRe=Qnu#C8-UHw&df!;ql*|+qouE z9qGV6-x5#<-+58y9$057wKZJE4*gHzC3d0@y}-DP`;rXX*n>R!xvx2J7k&S0cKt5* z;55}qA1&vIw7fwcg8Pk2s$unf#aJWY zoVt>``%2!hcY2}a0K7(Crpx(1$+W|E!hGmiJsupJzJ;IbAsAA00I@6~2lswqR`kvB`9>T3neeSEO?Ka6=0_~Kz6L}*LK9Zee9 zgYh(YGx1L|af+_A@D2m^w(tTP2ZLMact>ajm(+JI`h2{Mv*=HAOZuI>(P0Y(DEDMBY0$g zXfE(}>Pz_;#~_>M$T#?mBS*^TyA;(BsARs1blIeFa%QKKy4#L^S$}VCXMlLV2Xi~O zTpHBSR1&4{drQWCl=2$&I-=!1cqQ%F?r?R=o-|qi^v%ibY(y{3fN#nv_m*4OWoW~xtKS3W|-^92Ex~~FDEc$^dvufY66$lk=zOFohN)f^AqYR+ah-y zabHmlS2m5A6yWZy^6L|{HM3;^3daLeD{9Tugef>EpLmH(2 zMHfvLyRwTp`S*F3GlZ__WB)OX{YB14wzN~%az_?pu}uBEE@Uj6yi(v*z!5%KbgagH z{*6AzPUo7%-RIHRWPL(~FZZyPN7;1SL}2iK@(aXwH#&CiWsgn(yC--LatVC_bdboP zYL00LaQ9d+&!Nx5?s@|`=3HjE<8q8ATDS4n)0P8-`&hrlw)my&vkWkHmiy#JR;WpK z4O;^}xt_J{0Q(-Bsc*rZi|eFJf?Mz@?&7>E`1A5|oXqvkf)5f;=o#rB-ms7O!iTlQ zVNOGrL)j}KayS+F=xIwMTcW`G9x|YhRgpi@P47{kN5Q#{bC=L(MaDdELd*Awn~Lml z|Hf+xJaPt@?9=sM4eE5R$97&iqXjcl|5H132p4k+fstv{S11?t-?-~ zUR`_E6X)9$#=4%0+o10$Yi1v#)WL=b&cCilhcMnz%F(^Q?4XRrp#2eOmpcJ3QAfX{ zex1~c^DD$DXplBGp-}u@{GN zYccB=E_0umOQ}tPJcSMjyH+RjFxXJDth^V&7t*D$&u0U9mv-fzT|dIuZ{~=0==;r( zG3wkw|86_=9O9q14%+H%dbxe_iV_e^`H*q;HXV_b~62DpQe__7z zGBlyLv90Wv|dc?Er5(G9hV`Ka}+a2Y0I`v`iq5?4x-Y8J4hz z>EERdOIR}aq@P!;U;mbPFR~#20uSx{MrrNbPWQ68ouw|uD-X>@?WxW`k%#2}67BeB zk{@ZMPuPpUQSU=!T_>UCm`-PW%U|^PiP%u8;m7!8|Ir6)U+V7f*=I5N>NNGi0sGQD z((end!ZFbgO5cBzwd(hnqnEzFgL(Rq=oqm9lv8UM=Npqf?x|S^*L@hvGvuK3&v%30 z$K2q$J;)b&LgokqpYhG|EcDG<)}lV3|7~lgTrw}ICiI@`U9~Xx$m3tgJtB8#UvVT& zzU&ymobPb-zN~4DY#Uwc_F5u{mHvK z!aTNvYZ>Foz7TJVt@A0yeP!RVb*}lIt<&fG`pkWOXg&^3FLfpTbP4sDGe#XOz2WM}yP-OYA5l4%;s+hP zf-V~`vPv87{x$?1M|m2e!S{H{@P-oX?RDR=OFL*~jqpaPe^f*8Lv?VYnm`#7TZFeF zD;9s(F_u_|jlb@DVMo?II`|1$hqy8%>R=b`>@v7%YuH>OUy0aXzJ(s<^Nq44>19iP z%04=oGXax1v#!!Cx7Z|7Zn3p4Z&61=<1Fbf&i3AMM1BH0)sc4W77yd^WOU)|DV+C+ z{_r6KKY=zG(=*3&M8Yq@j}6$#BHuotmvoZm--%ZXFB%y)-h53RygJSGLH~X6DZMYg zjGSO&o+>7`_XfPNlCXRSBCeBa{8%mHa+fYv7i< zylvXead`q%S2rNS`3}dg&~@uCX^Hu(l%QU&kIM5BoZQLi}X<2q$%( zOngiCFz3}hS2`NFtV7G(pGW3RnD-b3Uu3;TbhuZy!#F<_nCF|uyy&~)XMVxY-W{<` zzbSPMPp<iN!`3*nFdSe~tG@^7(~ME9>RFOZq?SG_a3~u+TEa0O#8V`9`7h75oxc)+pYiZl(X0 zy7(pU;!6K1e7-?gX!*Lpxei^}Mm2bFYU+|Ts<~g0ngVc|l)9KAZ zhfw!+m9)vWY)wb*4C9l7L_$+P3MmZ$T zKT{S7PiBnrL;CwIBCps_t1emd3Bz0Nmi9fP_k4m+zO^TwM{l3QTYgF$X@76O7~UZ8 zEchE>EF%2gO1{BY!*klP;hX6X1LuwTgbAc^O;^+RZOiREOyAi2&D_p+iT5&n>5=g< z`doqyx{ZA`NiEs%pdDP}9X<01hp_{uE7v6ED2}LjSLaL(dzW#c=tSQg%qL*e*_eM0 zygDbxAE)1`obKbBU(tiuUi8N!S{!!Y!n=`)99mybcTa6?F4=) z{=S3XpzhlpVe*aTkw1LP)%lIc6Fa`KQQMijrmUmu7v%S%?MUEpSLZ(TLNfI|p1Lkp z!kQyi@RZD zY%6l{E^#D&XA0vf$}Ii--*|ub@}Qi+FObU_w$5hea@$|TCaJNr7e!@S`<|M?rJu9N zeC}@iff>2v1utscN1A^|Uv+t2PJn$*fl_yl|90jT&}q%kAT;ITW+M+l@^5C&*d{VZ zyZ~c!;r-Ks4;t6vRv^Pxo@b%W%JU5V&^&*@+_IIgr;Bj{WeHM_M|E8BEz59ayc0N2 zzw+l`o5-gPH<7&2gX!(8hl~8a{de45OTV;%y6|8ZLS!8`8jE72!ESCN#dWS zJXSjs_)COuk$KI*jQx-!H~Qha(1HFCeRYX`Y#j3Otavu-4VR2eo_8Gvj{r zWP{&P{D$z)$yi?JD?QqCAXkAB*8T|9sZP9MP&_3HgbvZjAosPAN}{ay_Ad zJI;(-!#(p2DS_f_$Bl8>G6yDlzz4md^3=?EaDp@UsV|@Pp(8IdR!R2zb~y3xQ?^Z| z_%AERrXb_Pec&CRmiM>()CEueJYFAXKbG(JjS4s-S~fz@Gf!!YE-y&gBy>2{Tl1B+ ze&#sjexKx9F=m*?cY!u+!596>n@XcyIhRj5vzaei{?$JQi&FSbF+5TYebJv{hE2#W zR^iJzTeK*}sfKENv)B_hkuYsJ=ZF@iq>+xaJz+*V&UhyYqL1c}T~1em!9mbvo~34L+rcbkNfiHX-{h%J3*>rKR{tHz734$nT%ne~_}7 zbg7|XX8EwiQkqFOB{a+|AA1>6T1l4{8fKR7EyDby%M1-O%O_!HNS7TNW|oijYS!R} zs+nr&H>ihw)IxFTZHt7q#b;bv0#*c2a~cprGM+-TR*Fhv;8#qQaf!* z`nb3Mj&%jvmYgYanKag!BJrFl@^^}PM462QrBoB0B zrnumcbuYmuYY;Lf6`bN4wAxxG6Hn$zB#wmXd6RyEnO^ROP>dxTdAHFQ2|dX^?yKkh z1@>p|6hb#}}P7X859H*x$J90~MuVrQ(@%bC#PL;oECE_EZ1LO!11Y+mc#`o2f( zx|hg9=1JngA^UP)ka-AMi%3z}7Wx>gErGmTQO0^ifH>YSRm= zR+-1qRM*!ZXAPu-xmWLfxt+hqma=iDKpE>K$ucj>nupAnCYy7k3Ybc=ZBo%PFRER0 zVjId>L2PQ}P^X;c_qLPF`OqZz*e-j8nKSfFWR8_`+RZk$8rgR+M-up&I(Uh;Cgn|L z{agBtSKztS7xQ|Y+s*w5$W0UJkifVkEHd%KcC(&4sZE}yx)dRSrXRp z1vAWJhKW3{x9WQu>j5ts`?8xP|IucB3yk_+MSVAi)Hm&~XdLz3EcHFjsBgg~_3Z?& z;F9|Oqu_cmH^;+zq-R57!=K=-o~PKSA$hJPo+~LN&+m{YHm9Dax&GjnZzO-G?qZC9 z{*&>s`atG$`sS~!{IPX^YUb}`Ez|QtV#CkLUt~)7C+pwJITYS8&wq!kUH1U`z zDGztG2%TQs>)?uFLZs4$O(KD9p8*hy%-{s>r9#SBa^QnlTs(%SVJbCH|bl0t-l&uU+B|V zBe1idUuZ1bX0#RgMqSns?&kaSya@G6YdyhOOQ6lLf3cKt`3pLIJ!=S)X!kAPr|0Hy_b3d1~9oaW1V_mE5q`+I5ACPg>!-M6! z^-^|lR+_c?M(WCl&vp%Xt z+N1E>ns1oM`&!24FGHt`v5&O>QA1Sc7v&piXqNuewfu<=DaU!rAuvDX@I#|Zjq3h( zT`^zzBx?Y()hPYjb@|@c$NnT4YkrA5WIva{0`i@3ipa736#8@1Lc%T)Ci1kL@{7(W zH{GSUBD111riAz>;rIW-xTjmviEfZNW#Q$$goU%8OX{Fozp&3Bllam{jTJp-hOL+H z2MD{9uuf>O`U<`OkbIkm=(e!D??VSAGnb!C8?+&N6JF~bU-l+R9DQFB@nj#7=-+8~R-PZ(-{y4Xc+jyPJ9AO!7d?OG+;j1VQO0%S`6hd?vF}anb>Z7+ z-~-}w<>mOK{|QMW-)DFjPt7uUV}<4XqW691JS{fX`b9P$VvmnD zfw7x8-^9l3Q%aMuT;7%%&IqaDJc4mr{Iz-X!|}{~XUMOHwTg|j;TqbwtgEe# zP26;L*p?kNDxNkLf32MP-q~iqF0}aI!&u~AzULp#+M{xA+hIfJ4d>fx`F{H(^NioO z$De8j-)+Pd{CZyK49R0XI>XANlze4g3VO)Pp?~{Z0u7SS64Ltkrcve`N8oSzf;w2j z84)6<(zp0b9pM|sSq$*Z^XTIpqU(HP4c*}zuR1qOq;2T?2v6wq+ENbsn4$o7vA*WV znHFLX$TyDv_8*o#@MFfd=Jz{h+j6p&?j>KbuVfr7;mOWK=3U}j_LDxDHlZ}JFKBlX z=1OeX13VWvx~B*3No9sOlj@SpnS+m3JhIorbddDd(B_Bxe2oa8&__4L1fJNn11a^(Aa?zw3w<~!*% z8`x(;z5b1H!Mls@WB$`f?^j{4o?Vy&&Y}>w54*z2 znE~&uqwezTIq2K%m*9cuO__slW}ZgYW2`xN(a*95YBTS2k#$hR=2oq;9x8XcJQL+x z*zdZi-JWxkeHHDg`-hHwcH5@RHSG5)P{tYPDb^Y2 zoX_#|;Py?FLwI{TF8iDsB<+8Jqd&f8Qr=ANu9I`3&gFKz?BkpAjc!%+A!$X15@^Hv z`VM13G(p}X`~LWLxBdw+CBL1+8m2PY|MgTADOg}h(BY(s|_cA1Sa%{*{k;j!v9m$|3FTHg`T zuQCoyEu@}BACkxAwd|LIX4#`CIIq0TeR67)j_<6L-IN`Te1z^Fn{+S3Jr$D2KEAOE ztaBX^UGgu;`@1vPYX#5!4>~sm7(>v$I^SoV=rU`#LhCDG&n{dCe!q;To?*T1{;w{~ zefFEx(6`)4da>hW?MV6u_B9kqn#+3LNiDlg8s_4*@W|dgDeFBl&gITC?>NpK5ZjRQ zI4BGD>T9Kb<}kqVACy^aPY?Ue0(!dmmOnyArb8XcKCcemgDojKPwLId`5(eB#hy3T zMmyg?pCr@P|B?8`tP`+y2VLlD^py{oH4YA)pZs9$X}!+~Z9mffZ`FQQ5dLxP$MFf; z&vW2u4rxDYdGD|NB(Qf-&hKyy?rA?wr`huauS7OKk88D`vGBV;xm*l9w2WyVQ^w3s zP{z{1AFm@G>{CWJ@1mC!IzrAU@zFQycL}h6u$eOY!>lvt6IzNX;|8;gV?xUKJ8<+@ z#ycpZ=&?Hq-+(*|Z*%*cQR2KPGSPDe{co5v6S)z2@iKQ@tG9p7|Gotdb+b1P`F=)Z zcZkq6K)ycKm?e&jwNe{4+)2`e*2ihecPp6_ee-cLm&ciQAE&eb6Fi|Z_gmiktB>hP zMtyj?&z5Jt>=ZU3@kQniQ#Y2(tskI1{vCK|8Ml!3R%K*i_+!iXV$8>v@w?y&E#o(M z@2`xn#~Eb|(608GXNbQ-8GlFFq>SrA%2+r+8NUWx>UF>FCbj&7?j}r*#9hu~>3F_> z=+#FrFlp+~R+0AnadrJFcd&h2U03$uLn-f;ev!7r`>Ef+h-brt8 z{ng#?fxknaQ|uUiEt&n9()UOo6p;APGz>od8G2++TX2bA+&`JF;Fk17#FsFU!{0zx zx9#B@(ePc~`>TU%&}rcl95S~q-)Bqwz4lSp#Lit!oWBA4o0;D)tbF^irN2Kw8UMvB z%Uj?&OM032xB6lqw&r@-k3>H#^3#ZJZ8!5+OMI(8-bL7D6aF3C7Sk{LfxpF*M%-`U z${z1f-1|)2^`w6Z+;TRW*aPzYlklhne#yAUoR@D+_KocR)}`$`e1AcDsfT9L`_R?W z4yabW9VBpPy7aTyH+nm0CjO6qhMt3eUgrE{PuPEwPTT{~=V9KfL|pb{Shj_H6F2DR z(jO$WY$EPJGV0XlvU>WV-%*Zl6JN?APw@oD>#dY0F5H__g$?FZfx|N1yMZ0$>?*Ds zs^SKxcMYlgU%}vXfy=oiQf}+qmnGivin7Jd+DhkRtIAfDIjhTXYid;wyr$M$UF$6M zt|+Vast~xh^s!at)n(lwv>JXT-kS2#;+op(@|8>PzPtE=;?l~Mysav&b*@}hQQ=%! zS?gTwttek?L{|?~E-tHZDwSJWTfVx?>8&pF*4$0lgOyHi2uLlgto7EGSFUuNo(%Q;u zy#yqZD8=O~Ys#u?P5g^$({qX!RTZ!BE-x#tSq9yUi)+1074Y=p2UgXVtto~TW%K4g zm{VN5bmgkz(lu++Gm7U|mM$-@s;nq4eWD~fY34NuYtcA9!+G+|=RpBiyoA>d! z(t06VmP zcurAT@uH04f`ZRiEQC#&#q-N+YKzNjX;+BaJgLI;(5lsoUQoa+{4IWbccA|s<5|Io2``Je8eBxZYrzFRyr52@D?vC^HznnYpXpFcBdZJJ6%cn%EivIr4`cN z5OcFZ^t7i^+6%3THnF&@Is|WN<%%kFjTzv4yu5aqv#h$BM&|5AS4H{C<<6?=%B9ub z6@6nVwX~vO?)-Txyi3c9ywyv~YKx$Nww;mx*@8tvCxqqc1n6<@FI*@!IBS-dS5=`K4XQ%xqkKhGC5m>*VyA?`Sv^Wc<>O`5Mav*;nf|`2 zs;bv}`O4KOrNUKZdZ<$JCP|AQ@K)&>=fvJw8Ptd#PESjVN`d)hEA`j8^NXp0V%Sn#F11rELDjSt=iPdr zLQfG@Rb5tEuE(ZSRn8?9mEPKW`T)yU*50$)Tm1wqa-xGXoRp@tva)*dJ*!rhFR83v z;jHnlsH!NdE->+vtHilyb!GWt=L&E6%2e@9a;|mq1H&}rw4$<_euG$ssHEbX(v5i1 z)as=r_ekue6~%L^y-V}V_$1Cu)e}!GSz0~CnKtR3r&X`=du!?EN>KI&sRr){AiqEQ ztg=B|+SKXJ$<9;*h4T()`c$$rXj)KNQ*L&BI!Q(x=$V=}#fbN`>P^@Gk$Ta29U`vX z6{ZJ-$ZdB(No8e)vs-|>!2?yW)FoAwv8-k_)q+)FRBx4D*WG|jXCds=8KpDmPB`hj z%Q;P$mC@rBurOF6Ix9)BqI?b9r^c(O?J3T}sgFN7X^MfTYUI zkZ_}PMl3ObAinI*^fW@M%aG}nIww-SB{iga66uk|ppn#+JvEDRPX_%R&XtqM|FZ&< zjO1PNSc7Dnm9fyfa%q_`Vww{^hp^VjysWCGyh69aU~XDkIuw-XkxMFT-BO*ioJ)1} zi_Y?{EO=~{w;~lhQ=B1!0bwQ@DVmYIwzA4hj#{eelQu+lAUPy$`4VSpxBQq*-egE} zs!0*WoH!BtR>x0CzXBTLlhjP&Ulx*DC4k=u!;475Q=FW+C6Xtd%L-ZjNF zmBmZE(k;@EDVZ5izOp9(B_jd!ok(7-vs~idgEP~aiF3ytW`iSw9{#8i{-_cDXm_|? zbs1PyOKLE(bk&@Zs%Mv4j`lHpk4_Rr)th`_w?$a#s$2BnC&AyX;af_U~7BY(~2^$*byN`!f5Cb<~|ti63a>FLQl@R9VBR`(Bm80aey`w zVnUnAyIV!*_(#Tv6^x}q$4mW&OLjG-UdAoPpo-p4DRUfW4L)lZ-+lLDnGSJQ$S|?m zS&KPhXod18%Tg!xGf$imKk2`U8N7$KH;#i4<0Y0x|RBr zm>5-Q8EMnfGSjA~Wu?tX%TCKlPfJfv&q$w^o|!&9Ju7`idUkqFMjE|XM#i*^%#7(7 zSs61jvNLj~rAF)e#qPG(wWdS*uEw9L%R>6uxXGcvO?bEcC>~O&zPP)Jtr$ID?KYCYg$%j*7U5btQlF^SvfP(W~9%^m@#ce=8Wkx zvS!Sfkv$_PJ1sjsJ0p8qc4qeU?5yk=+1c4SIgpq`_Bo)=Az2O}i(ji&E?-&s_)2pS zR#V0Nle27PY2{*b47{kUwpPY^%%3@LU*asOpvrDv5(R%4P*=&Yhasf5v=pmQN4dgV zrH^RuT3ojJE^GGUF2>!f)=Vi~;SHH@pbM{ZR#nnyE=^4gc(ZW^C@Konz7L=El6FtqG^=C?Vcvaxv!AX{ z#}=#x`|1MblH@1l`ESLW)`K{2Sy}0F;oAzCsJ+{HdkvEs{3=&ZN1Bu>U$G`5o!Z9D zoWXnL>YR-1ba7{7Ovlado*66C<=d#Y>hilvz152e@AaOh)aO@vOPC~LKH6-`bKMUx z#j?7*)R^Ju3707wnT28QjTZWZPL)#gRx(y*e)&o2Rzl6`3J703_4dV4A^7$AFLX)O zvhvazrd48_oo1jKs_LH@Z9CbZ<9Z7p_KlHm>}daP7EVxH3a0 zaNB6dJyi~+WGedTxM1*R1mU<-Vw9Tc3I)CBnN|OiAp_!TYn8cyccn$VApY3|4?JN7%mle3hqqYnYasZAHiLWTY?&@IjO#HyHHb&U8~A-0Qeg5ZTRn!QlSE&@nd{jEq*wHJ^C6yDEdh z0Pg9p2ZN2^8M77oB;B@P@Db8gG(k6R^iJU7vWTAsp82~;k9&GA{33lYP zcNcCe?jhV0xB=WNxMN!`+N~688}9 zMcgjjF5Kw-!C)M7z%y~vaIdo%R*(A#3tlI1PvdssMzdHp2Duu8TY>9jL8uM)5Nx2XI?)kK$g(4dANpQ7^bNag&BoU$|3nE84&-@3=nPUAT?7r+*L( zcHkc6X8nO_&qc#Zv^V5Le~KI(KV(9PTal2Y>sUv(y4BgFK6Qe8fS#yMKc3 zDV_`I$ejrl*k+z{z^u3e+s|_bSSqgk1a_F`dY^cwfnDtb3jn(UOv)oaNqdv$2C&e4 zBT+(vFBCQgnBcQul7AX7!B>UfDsTQ4`{$zc0_P6SHw*40_z~hZ-4+Z!BysOIcvJY> z-^41n;TKwb#L+T(^2i3(2uuN!a$9-8(?;88C6{xtDtGXlZeKySPcPdU($$M)S^GkQz)M%;uOyW3sT+lrQ1+BdghTK@s?&v*(-CHBK zMdWXZY8xo`3xfgB=QZ~VVm?tTP_9k&R=yCNt@+FVFt@drU&x5>&CSs@nG@T$# zG=BLxVZo@39q_aRm2F%2`Mp z3wIi@1;E;nkB`*d{oUoZYB3Krsj7H3OJ3AZlJNTsN+~yX!S1L%gWOvp8zb_zhHneY zZ*pwcDlVMQ?Ji#I0tu8bi8;R?_;#(yAz=%nLeoi}(tg=ft1eDMCy}>Rzj;!%&ucpz z?(N}CVR~)5cj|Tb>ot&hONqv(E36l`v+&%WEaXPkS>*v8x3TgPl0 zowt2d)5zzOc8qZEOx!ixG1>+PetoeQCio04z7dFJx@I@?XOU{D-FX*nAEwVKTeW(tMqt!-O)Dn`W3m;D1R5YHjDrBdK}m8$UPBK zIfBQ%g-({A@M;46Z`xPUae_BDR*#pzCA={#Z>zQqX4&mhx!sN7fzX$Hl88;?gq}x8 zue}Ee@XEG3ZqJasEwPO;?(I=cgYvgVZyW4>K5|EdduO;_n%*o|F7O-%Pd0YJV(LtL zOUF~SJ9bZudrNd9jWcT7Aouo2`}|(nchg0B;URZTs)w*8YVm7(bsRbmg@H!7?Yh2B zqWFi1*GfF-HQK+(N4_Y5ys*OXuz6v5VfSfa_dDES^X*{|*or_QG7>&N{DJWM!tW0+ z44-G|s{Y_g+Ln)eCTyZlHsny3`#cfd9^ub>-oC?D2J%qRnL^r=$jxDB+>c+oOlNJ@ z4RzNTmcLc|f)HdXC>L?AlUH3mWe<^g1bmmxEd|T#7WzDC1GOaXBdznnk%rB6;-9cF z4O!^J7eO&+hvRu|Y1FUYq_Ori8vFe_?GOICV5dDl>eso0@B4M$PP;B!KJX=M#ufl~ zH~qR;Ng+h8tY(+L-TsBB5DhR`?~hL-o99UH5}EiDnkZCv^ytsrZDE|MPZnLoztR8? zL>7Oo$L}eF(d2bL=xSi6eYtQUl`yDHrsX& z+cVU?C9ZKu{?^!SG2ZRbOV7{Q9$j{R?)K>F^YpAHt713XB$CA09xZ7ekZ9;?DL;5G z%*OUf0q-dtZ>xRx#67pWw@heEac>>JE!n-@*)%Ty`LR20%iB3-*J#Jp{uGz!dd?P9 zNgVW$D!l)*u0wkIJh${mkA~Erk^fVqX)B>`mHf}^X*Sz)yA|ggQJkVT^F(p#dh=yl z)Ng*RXAH;(4ZWMUMFiW3GX~wB zgxxLr61lSU@*GPq3vRy2R#(6cI{i0>|3$z8z%m7r-@D(GaK0T-HwOrJ65dF7TqENd z{8qlu)o7!e6Uh=Bhl!I$9MR9Jz{q26NZ)*c@cD#0C7hqodL7t9z^3RB1IG81sutJ; z6DDDN3!ydwdqiNS9h0|3%MY7p3oEqG51$ua=nmyUKJi;aa7bPc0rLa1crG+o06Ph6 zzTh?O3Edv#RnpWG-a$B0ssAKRGq9V$behdNu+lt4_-X1^V{CRnWN;b-g520<`|b&Q zQrug{Hzwyd-L`#f{!Qb=$?I*47NQXcPNd(ur{Ko5^z+?^ViLk6%07N8ijd<72vPwa<%sT?QihP(f0g@(-PdPxSCl zc&gxT8!$^woq!U6`GM*6VAi?VSfMhXO`Pk{5}Izl2@6eEu*FWZG{%CGpN!!(<^)31 z$#`C1w@P<_IAe(80*CyBA1A5%LSR=#_J2r(&@qRuf9;Ee=jI?quL^6xBDBanK?m~> zmOKee<_vBE6M2-MX3uE%_Qa;)?k%GlM;2^N+BV|(1bYej zz1h?28+vpxI9p%+$QW4+HQiv^`?b|>x`?%q0V+fa8{VU&AI zLSuaX_PC}YdC$l0h;i?X-Zj{M9lCp(K{&HCxr4>j>I)Nj?+eq%iF}i)(x6fAHzV|X zKepd&BiiZjQrDvI{tUCQp~Kv}7wq}0d&`5356s&-f7|`f7w(vwzx}?Zc{}IqDsbfV z+iyx8-{fD_4}1EoOyLEp^E5Qcm>t}@jqesaEN{EL$(AR^n9jt|x?Moj`S3dvT!%yk z|5c~U7`w^POt)b@u)z{Rv9c|Hu8irH$lz>n;rR>t8~8jtvUhRwQEW5~oIV|AkDXs2 zc7ERT;XA^1YIdE)Z~n?c`9M>|_Q)j{&b!53rlYpI1vjtr&tK5N;CJ$LP~M8cX8+mK zx6iZMs`Q6rFbk5<11IrYvV`tmVZ{+}Aw*exvnFpbX(U^$`E2Sb zJOD;bKz8bF8l#u}HfrmrO}l7Q7sN}b^~6o#z0M|>HrqKURN$v=9<6D(x@e2rJe&cm z>*f&E>d<`g>Y_ud8_w|jfQ{+817Vp=-uc4o2K* zABbv`NLL1D`iH91(W!uZF{z|(i@UYNO4mzm19`w9v!6z+szKZ|8>AnfTl6K#3cDz`o1Qoc|5Z8I$+YxFXt? z&BM@Qvt2tEp$^zJ{AcW*jp54g(5{553yw7Wjan-3i;}EEbDoY+hr_fJ5$Z&k#>{(1 zmYaOIcN2KB37^xAZ zE>go^A1Mg@kw*FcGrY|q!~1g)^|q6F>PDp2k*n$kY4{P81G(zxpj7-f2WdA8R8y1| zC{U-PJbvJVwf(v3++Yp=p=i(M0<|kf^YhVhjE4VQjOTEHIuWb2xz)v34gby9%w2Be zCza5~xKvVIjjOlq%2gYOX>}wXrs3Z|ES0DihNTj9f4t{#zG_U+PUovb2^#*>37&c~ z9ZQ|R?Z=U>cMbz?lfT-7l~yP2;x-=^Vjx-FH6S8nrMpp0X+CbzmiR>OaDY$~Dt zah|KWs@19a-0GxL!+*w^O6aa+PiwxaAFrLsSNq3n_*=)Xp^HfIY=)W%8sAQzo1o#p zI3blp2X6PAA*+d+pNJDR{QimOXzAA{Nz%s20y#I?b0b%sxI^orBzI`|yYBFGOzj*bXKMIcGj&RIzi+yywLsmNt{o~+by*tz%~_J;;Vi))$P%0zXGn$bpW$hu zVzaf21xx{H_)lkh&K0PmIocV-AxFc1GsklhrrxccC{SH@r{QnCClh$xOwWF5Wv12) z{#hFSU9&Vo56#NNe|=V}V7^yFCU)JM2@QdJCEuI(N-naBRJ;1F;#RZG;s= z((oUOlgc_7ryWHM;gq5J|HgQa&!uk0Yn$h&`UDOC zfdo(89Mv>jJM2;@Vf=@Orvk4}thXIPB1h2b$>Kj!+#<<6%3oli!$-0i1KMlk@{#(Nd?dK6pz1v zwxpd#wkC-3slPoHY!`0#Y=-`cQs04z8vd&jQ^D5u84q&2KUHgj+EkJ06RDzaHc#?g z&sV<5T35bmnyle(o}3D{>ytew1X`UB{C9|Q=(-~nY^_sLN!vBWa~-vCr*_k=Hcr*> z*H29)t$%7NG3)R0)aR+LyR^DI)tIK?Z%(7GPp5fy%~nU#wX3sPvC{BgOqZk^GbGB1 z3=fT^ZJKt0<}*zi&ISI@RyU_f%qyAFOwLW$Hs-N%EX|}o%X7ueDzN4wlNlQRT{8rJ zVuqyMnC;m$N8O|`Qu{d?{-zwsYk!U=Qj{a{F8~K~j>yK19HF7;y*A;{B5(OGX1k8_tUeaqRs$^W*rjHoA&it zLPN)_IC8JMS8}YsS7_dRuT%)l92Pd+E6m-nXS6;&k^MSo}(P`u6EIaUDBFqwJz^ zuIH(ZZtY5*+U(XE^Hji%WuSHyXl)4I9IbJVbkdPUT9v9WY>LD+Vu@hI-LzxCC0wwp zb9TeN&~2J2+5&7o!=exyC**y+$iX8wZ7d|8*2IpwqG>EHH%8Ql1H!1%Q^m~3Ez+K1 zoyPz~J3*hX)v5(E(Ik2tp=pZj8+b_Duc;=R=F`+koA__ov}R5D9NGm3r#(ah=@OeM zxU<|=JVjR8ZaB2Vv9^t2QyOE{$q4OgEb4{`7qJjxRcn;bb~;wwhyijlM#I}7-i@*1 zt&7E59~()$Hu0W})%-)$At4(R(b8s;m)G{CHk-y;&n`@v7SFu`S zY!rW2tO{t_(HPZ)g%mBeQFF9v3$Hj7r8=U(wh?P0#&$SH+Yg;F+PN587m!#>XWm+$ z79X%_T|*`NLqk=4nE3aHX)K^`4xf*SS{I?69;TWi#B}vXtYJYMqo{6}>KG#Nn}$Xb zzj>$_vj>Js%nq@1o8z^k@#=8AAcOsh&Gtt&JA~6P#i8LnpffsX`>fdSh1!1XHrsww z{69JX$+9QZ+|w_*=>ma^O=A{QsK+&aW8d z^F3?0e$!RY8Sl-e8!%mGodG{%y2cy-^$m-xFDEg9!LQbIH{WKw>p=b`nekf9@Drxn zoYgy?75`KB|11ZpWHWHc&)?_W3Q8*L9nX6EMY<8gdjEBw_y6ki9-h%V{;)pp$$j3_ z`@GNX^ZtcC?~BcMwtMKmmF7DuqWUk*{Gg3p2KvwSS%dPC=KF=`ne62^+I-jd6yYCb zzQ^q_!etFdevyWw-uWXH98gr6e1B?lIm?c!6{VAH33_>>RA zfluY&Q#trl4*uV32Zpq+d7g+%*7v!2s|ybAH9j-OATLK>RZUzIbO~kxvWmGLsqj*(Gj{r!8z&zIA=_wcySD!sFGl zKJD6yf2QB~F{-0qc)VIO0G`y(_2z?tKVEI_H-5Z2FaZ2yzwj8XGZ`j77!Abwh(9?YWC zH$eO|w%&AF@tX&VZ|^_;$pPZWX}#lHeApb`KfHS$+${EBqKO`};4Twx&^c6H9YBu- z`p{#ZOqbHaLe>G;wP!Aed1T?@q6Ib{M{c+_zg3@1%CuJB68l*2QK^0 z1+Knj=ou@11`~_|kL&~AYr^CDz-0%f#82u2f5e14`@j#G@YFu=w@r9jA9!|{fj_$s z+-JgFec=CQ!VCMrt#-Y@gu^cVXYhH5dZ^NZ81MvNznoAEVBc z_J&*Wn+L#St;3jPs^QQha%jOD%>V=cP<5kUIZRRyndPHAX8bd~ z@DEW7^usMIEJ{7Yd~tvLvC4aOfbs@r_RohK1Hj{E^&j6_Z?Y)5&*YB);|2#t}fm`cT7M}k#0DKS(l=#+qmW8L(gsYJT{VtPGtKJv%DVN1J)n}b0 zj0t(cZ>^`vcf9g5;>W2Q&-Krr=05mKO+2mUdYgP#DnE;!K%e-3vf@uMBFgu%@*7UQ z3q97loh3gOp19cq(DR&;-uf?RHRyOu_)IgvlIMXs*dPqX1T2GXWMdKs?9lu&5R9=Mk|;@0qykh735lTsmn-TdId}l5;Cs10-(Xz!zbIa!>v}9jDlhwA z2Q|#|ve}xtC+)w9{t;ZbB>jIR)6d^g^q*k~ej(F;h;i9>m3^9R*vLA^LY&L{BZ3S4 zZ2bE+DbIfC)lC1}jBm@}n$FX#EvwRVUExM-n4L@y+@IMW6gk;8lcuBTUub>c^=Vwn z%RV%B)7pi0Nqz*+%8zC6w{OklA6lS8^C9Ke-pS(?{3`xH(|@{ke(rcTTQiw{N`DHkN%!Z2kI|o7ZkL|u z`~BRXEdFuEk7dg1c8BvMU*PiDa|I6rCp#ti6X~s{Go0u7o+>|#Uyco5k`p=aAfphz zW%EccK>2g5;rx>DZ6V|5Ydr3!(8n)S<<74ze}T0vMiq7yl z`Z41Yp9-HZ!EZj0{L8o`z9=euI3GBr@ZtR7^}x?TelnXc-J$S{2kTTzr)5~x!_)$CgvyT|cf8Kz9&48Z=gPYgxX5jdVi=pc} zReqDTjs3lcc);`<7SvCPyZ?&uqXznKWjZI0=_X)~)pguZ{!73WA2@&5;qr&k$-Mrr zDtwc5^d4O>;_`PG%6|wr{taFKXedv=RZIPmd@RB`b+ynL#RvBDWn9OA=k@c?4ET2p z__J~T&h73}IKN5UZUUY+uG@f9KP5kKHP4p=2OzJXZ3BM00e`0f|04tb zA>dcQuamr-_;n8(%752@UxeT2y)rR=d07CSH?HjpkMmKKN1*E!20E_+PW===T>AMo z1N{#G&*T56RryWU(tWx;`Yk}ZzGf)@1pL-;UjJWUz;6Oh^DBAmTX}x}n{mlUeP1_g zjo|=Q9{m~MdHt^fzXE=v^L7Ki&w$qr`0pCyI$G5Tp zzggkXpX|rp%5>fbob-e2N5E{U>u(HnzGT3kcv*gbo(()tzg-8M+Liqjh_|{z1D$&e zbUtn<|2YHxzXtqS5J;Lw*~gT5+zmXB&xaHaefVT853*kFG?f1omzO-G_<`p>m*_+P z9Mj`Jp>NjJz==PSC%KyOIaQwDByO(+o~QTT#Po0ZqApCoLrT|Q80dV@fN$B7&xdWm zsh_gHhPUd9@Pha$`?E6M{{T+qWnXDZ7q^O6FwXI^$ip)5+;IUXzHQrkj-v1Y)47xJ zuRl}cw=(|s3g zleqmcaB6qUJ2fct`wgaZ%X2jUg}(Lk`Te(n=gsdy;6z`}CkXvl8R&e{fPYiro2aC75W`xy6!fVf5?FU0C*msx4a;~-K`48{JvkeC+XcX@H{$w z;KaAA{``H$m@T^pEH#Ih5_Gn zjh^>MSZ}|X$F*1C{3daGr2%gNC%!$x`e8H6+ed+uza-}_L?3>|K<8mq{u$OH=c}cB z;ac5K$?MW@Hq!MH;6z{Y_EP?4RbH1#{_g{)e#-tD#8_QxjLZ3%^BMn!?%%+BZA3k^6u+bYj~ z?Ka?t6b^mD^031FzuQp$Q^21-JY4I9q5OXVCwZ3hMKH(ex}=y-XAkha{=W-2@iVKh zzOKp-$K8(tr*`EW6sB8U#ZgU1&Xerqb-e&QuiYCB`0p9;;|Bce2K;{v_>*A}^ZIkC z!XZ~IpKvGCRWX#WN%^~Uxno@ZC~(D3-cK)Rb#HylK_Q z&V>@UcL*ImFDG;!G?f1r1O7t;e%_?+zqLtE)IB_3*DE~H5q`Oc%gesQZ!lg3PVEa%rSoquOs&Wql_ z`17XqxE|#5PO`nR6?k5|t6W~r&0fuPK4B<-<#qXVUaas<__0?#A-6M~kn!W}$I3dr z7x;Tf-uRr}gHE=bemS{Rzg8->b{< zGO!+IT=u_yj_F+XqWtkr8}Qu*{FT5--sGH{d>!!vhVmaa;9peuChIEZC*G>-f*0ra z^Ad$a&+z)al*?bwcsAd1yMfLf2K=+Y^YrIauFvn!Rs%k7z+VnLZ(ZDKD1Vm$|8s>8 z>%Gqcr*$glbwlP)=_OjOv3%^d@z66})EgO$~Tj9x)`t|wS^7(VE0lyA7 z@mbC_iJaePDDNBa+YR^!4fq#;EB`RF9$zt&KQFHXo~LKtZz%s6RUUeS<@x27z8<$d zpZ;@!Z{ae@+X(QycCRzwI}LQIhVmT){;LYde&QeM-q7=b=z1&T$DeYJdN2OuXBe-) zNx{|~`evOs%W}y25NuJ`0&tS6TbMsv7(Wk=T;6zJi}HEv<-LjyS5DkMYAF9LL;0u9 z<+ppW!l7^3PAza7i-z)51O97FC%Z3TZS8eKJU_U)wNa~Wb=|PGGBI|odY?Nyz!Q*&Eh z=sI|0ZMEu!BehP)a&E%&fk*Wdhj(b)9uMic*WGBApQl|~+&hi&>DrU~^@pXB?W1-j zT9|Si9}h|Pqil1HrnA!RJM^sL?n=!Ao&qD;JBXoH_#1KnFWL%Y|ha%hKb1ip!Z+Wrlw!Jww za!U)X(Gs4)Y}<=rK+lC^FL5 z#%fP`jpnR+Ub!0Y?w$M3&;avWIzywH!l?N1%V&C6`Ay}I>m;ImN<#P3(A#hi^CvKG z0iL8=`9wC}mwc{ov@`isHr$)yI7wTz*VqFU0;Mqv1zvS)ht*N3cXh8qU)A zr_rk4?gxjp8xaqGANYfDW?9QxuV31``#`f_Dmb;K^mu|iAGdDX;F=qGj^FAu$sw6? z%Bq2|3^yg}b(&szmyxU{iU}%}5)4VJ7wwr5v)Th6YpIeYnJ)yfXP1AG&np54;(@8hUfrF=$aH7Mfm2TYOv{bKP?KELp zNzJqyO);DHXf0?j`;D0>3jJyq60~5C0gB+Q3)bv<*HT1+D2Q&*mwnFx=(thP@@riS zVOKZmEiaGMEQBRq!|z63=tM27-k|M)s8bJ|Ml-0oO{b1nr0cjnglHTP)kg~XC7fk6 z=cvfT@!Ct zqCe)iwO2Wz*R&LuEX4rJnY(fC%%1WrN-yo19kePnhbB%U+I{o$mC};4G_!r7MB1tC zwzeU7+dg~44bJGu7!ZiQZTH$fLRkk4Jxt2B)q|9jCVFiGSWO3`onBO`rE0^m3*93# z6mOD%CBk1;k`O`_JxMU!qEf|Le{_!<&Dvya)77R(K*ONfovy$DcOYV2TKO3>JvVS8 z#_JSiO9X6_=I`{nD-QXWB#Px-`*%}l+@I_0TXL+H*Q#~a^bj2U0L-MKoet~Ax>3;4 zW8NJrecPVZJzKyMP`z~hcDHnh2vnlts8i`UwV;R9rJM##TBqsOymEpi_V{)bsPFNj zm7urIkd=1NsjaTsMP~ui;B4ha?D?VocqyUI zC$xz!C8c1bSVW5|qkv_JCI|U|{}<+na3Hr~Pgju-2|DC@K|#4SN@9#1S#hK6xQhkV zf6X$eg`frv7z(K&{!DAtilhf}KFFL*Yc&E9fx_)+Ur)(xTr;;!5j{~NG(1E0MAwUw zsh@7CuCA(ub=b|waH5?;CnSF}^1bf(9#UP~$4Q86)gN+got!K=LxSaNoQx)ZwXh6kP3 zUGduu^+B=V%z4XhuNmnN>*`baXfgDm^+S(phO-Gjb&q!WZl^&#_zj9b|9+i-3B$#r9OwBPF}ZL4%|a#p)h*!0>- zg!JH0W=&~PSOp<=*jsb7&P`Wcf?m&E59lpSA=E7ld>PtfA!yLLjfXtY$x})Nx)22v zu>F>upSWUS6YsWoVsVmh?|#&JgnNw$=K0fpLy2$gqL=#x@E zsh2U1b`Z9(B7Dp+DU9+Wid802QrjD498gRxK)YCVyME2V#A0=nYv_hugOw8juTckT z2w#u}_FwIlS~W}!bg4?w73U26otp8qq!V+ODj*6S9or%_>6K4zO^EQ>H(?Y$*ZpoM z7&byFTRR3R58_XVhgM}s&ttDb=|IeXp3xIaHi*mEkW;EBJ^o^P{P~7) zO05lWSDC+wN`w)$NB}cKiG3+V#qkJIp3ru3Lzsy|*6~(ro-*4>UXr3}YZKkoTI)dD zU&acAdz478*rc%PzDfXLFT^J^U>Dj!Zg+Ff=Sy zF`<1lOVggP9cs{N57M?_x|ObkBb&b|WbJbMt`md}r?V17LA&pFDPI-3-Hz`p_*Lys z?AnXHC?zcVJ+*b#S1M-%(QZLhGVD?#TG4ALvw*;$*%hyLIKd%G>h#FOQs-mW070I2gH?L;McbhH3BSvEz z)x1iD8m+w+)JAg#xy8Q6PA*MJEZDp>SxYRU4^B#%m9*IMdKGIcrd#^~I_OFdPZ{r1 z(d!AZwx{(nO*yGK60X%5v@QbFS9{5RC`W<&?yGEhl02^gR+N)Ec4F=xAvP_frZI>SEpW z{VtYr=!R=A3&O)tBYC=Fv>Ul$G}ErXwCDApn^0!yph1gM{Yo&t+(d|*)?%vGUMwSW zHQcJ-x5>+@HC?2I26162$s}4cL(ZgmYl@nV@!H)WT!QNZd24k_Naw*14c2yY@^Jk^jl6>< zrh?1;?vadW6e@lLx`VW(U4VxNW88*8rSMVQ`0{)eJ0{3tP%POqQpz;*#H@Qhp>$??QBYb%!1)x1AWx>fXPr;f3QIOR$Dt0DFQ*RjxphT1nTf)p=IlAyzH9 zP8*W{u)37D$k?i35ALTt-A6-;$DgmKAmdqLA=iZ`XU9g>cac4%72cBNV0K9b(>(_%b_%v)0(63w1!&6(Yp7lT>W8+Y~Q zV!xFLDn_RDlE|dlh#R_7@up*D8BT)-``YTzY06JDoZBg>tiVcu^uTrbilv^Yeb8dpYCl>AG2O)n@T=TAjTh|3%6npTrCA=69P3GGs9x_B8|kDE$)2`?e%A8GElxq@K^4ufziIY4{LXzOEV|CJ{aL3u@3I#c+Bd^NZW(Sl1=kg$}bqPr&1oS+7buf;K~K< zVAMHSw2MfIx^?U>lS4Y%4vu8i+w{N+vmmSFJvq%fDS&B#9*C$N{`P?&>y}uGevn)W z8WjGaIW)BBlHg(kRvjfu?E@gjqwN57T5P#q;tev91yY<{wRZF|-+2B9ZVrhDqMee` zBVfATZW>*NxY$VC(rhdRIuCYg`wePKYrw74gs*qIm<~c<=Kx|~YPFD5RijMD;ISK! zw>L&HYt~NbWNfSi^!dBCcf`?qKS`%=3K@Q=W1P0?yQyRv1f`qRTdDbBYm-(INvAeH zyQeHzdW1GM&5=sz$~lJmDprX)M=;p5g1&NsWqk@|DECaKcH{Y9N1RZyF#WYw+Syjy z5+Tla;c!EOL$qAAlStQ2#&q%^^S;MP8^$~q~q`bM$%!B>D=w#!M7Wx{Gh8W z@fdlG*{LTI1K)8d5=w+_dJ*nizn<*ngw9^lR{nwJM$*K} z&c>aX0?#25^K(dVL2W2`Nc1rt%ot8ka5jaOa%ufj1gXackKXATq}GXt88>8|-xNg6 zN$fh)$pod#FqzcL%vSao$%XP%^G|IKE#>6QoQh?e;KVUBA&m{a8ahRBYP+qep3=b! zwSK&IefSuKb5pShr#7fV3Z*ZWq)C75ZQU^9b?S9{P=yh|Zg0 zjCvjA#CkZ~<3xU`U!v1V2>l@VsqJEegOhXWH>n=T0Xpm3RO^>=mDGy!$HNYeU4+i- zg?uzaEi`*t9R*zXR6(xdS!DAYA0A1lK7BS1hiK~R^bBs$!vTHU^)#hH_fZxUoBUDa z)>iVe2?Kw7!%3bc`4+GEMI~D5w5Bq~YzzW98+6w6E>yQvRY~WR=~T{s!aZECw-D#q z67FNZq8Xf-ffJSEAkxHHyT|~#=SE>n09xq{civ0ProK`_sD5O5?t(KQ?Sa0zC z4}1ZEzWuR82dflwhRzXa4jpJ8!C5J1zt`aqLyP=vuR4o*Qlna2j>xJTuG~i&zJD+ zsaW=YHas6V4eCo2He4Hg-2kCcyP;j1H1j2J_UrKhHjYmIL58%uvH>@QGk)ErfNZ0Z zinf}Js!En|#v&cyWY;t;=(MqgA;o3}`0`k}0LvQbuC?&J3EZPl-&f=nPM3Nk_`c3a zwdXhM*ZOr!y`%3nSR?hdHfrj76zWg<_zoByJ5$B@jf2l{pu(jJ{HoK8tPx5jTO%rU zGD2UR!)E~*ENjGD;jhZA)KOR7foKhftkfYYxT&P26%G9@I!n=z*F=XJ1xeLG7hn5B z{QwQ(zY*2GZmSB{cm&_x)5f`(_xO2E>${IW z8c6*|xW3#keR2>Yun#la~7yZjkyHSuwzC2%D?vYF!lgYOK?o54o4!hhh<@w|4gZY#9 z_hstK^V;QpG{gUF`yWC5r{Yh#DbM5j^4o|jmPP9`d5QeUou=n$_>=nb{PzpruIo!V zp&{?({&{>rC8fSRCvP*?m)}Xw)|dBR!wZsKsV~n*D!fCte+y5nj9c23dHg2oCfk?i zA{~2|u788RNpl1K3!QIe>dW)#AG%Xle77ttrXcm?{{2k-WBi=hV|VNN+qj2Hwg=5c|8{!9JGqZw+cAOPQ`K7I~8b?IK|I`jd(m$j2D_L=~w7p@z2Idan;|!8@%{G D<;kP= From f7f304ca7a95f3db01e11d321a77ac333cde0d71 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sun, 17 Dec 2023 07:27:41 -0600 Subject: [PATCH 18/20] fix bad implementation of global variables and version check, which prevented python version checks from working (#1050) --- photon-lib/py/photonlibpy/photonCamera.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 0d474a2434..fd3e0c8660 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -19,6 +19,7 @@ class VisionLEDMode(Enum): def setVersionCheckEnabled(enabled: bool): + global _VERSION_CHECK_ENABLED _VERSION_CHECK_ENABLED = enabled @@ -131,12 +132,16 @@ def isConnected(self) -> bool: return (now - self.prevHeartbeatChangeTime) < 0.5 def _versionCheck(self) -> None: + global lastVersionTimeCheck + if not _VERSION_CHECK_ENABLED: return if (Timer.getFPGATimestamp() - lastVersionTimeCheck) < 5.0: return + lastVersionTimeCheck = Timer.getFPGATimestamp() + if not self.heartbeatEntry.exists(): cameraNames = ( self.cameraTable.getInstance().getTable(self._tableName).getSubTables() From 36ba8b5263d973d392c3757a8519554359b3d364 Mon Sep 17 00:00:00 2001 From: Craig Schardt Date: Mon, 18 Dec 2023 11:42:43 -0600 Subject: [PATCH 19/20] JSONAlias on old tag type enum names (#1055) add aliases for older enum names * two more aliases for 6.5" 36h11 tags * added unit test for missing @JsonAlias * use proper tempfiles * check proper TargetModel enum --- .../vision/target/TargetModel.java | 3 ++ .../common/configuration/ConfigTest.java | 28 +++++++++++++++++-- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java index 1cd3579382..6d1e6985e7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -16,6 +16,7 @@ */ package org.photonvision.vision.target; +import com.fasterxml.jackson.annotation.JsonAlias; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; @@ -108,6 +109,7 @@ public enum TargetModel implements Releasable { -Units.inchesToMeters(9.5) / 2)), 0), // 2023 AprilTag, with 6 inch marker width (inner black square). + @JsonAlias({"k6in_16h5"}) kAprilTag6in_16h5( // Corners of the tag's inner black square (excluding white border) List.of( @@ -117,6 +119,7 @@ public enum TargetModel implements Releasable { new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), Units.inchesToMeters(3 * 2)), // 2024 AprilTag, with 6.5 inch marker width (inner black square). + @JsonAlias({"k6p5in_36h11", "k200mmAprilTag", "kAruco6p5in_36h11"}) kAprilTag6p5in_36h11( // Corners of the tag's inner black square (excluding white border) List.of( diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java index beaae0e4d9..ad6db6fd12 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java @@ -31,6 +31,7 @@ import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.vision.pipeline.AprilTagPipelineSettings; +import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.ColoredShapePipelineSettings; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.target.TargetModel; @@ -132,13 +133,34 @@ public static void cleanup() throws IOException { public void testJacksonHandlesOldVersions() throws IOException { var str = "{\"baseName\":\"aaaaaa\",\"uniqueName\":\"aaaaaa\",\"nickname\":\"aaaaaa\",\"FOV\":70.0,\"path\":\"dev/vid\",\"cameraType\":\"UsbCamera\",\"currentPipelineIndex\":0,\"camPitch\":{\"radians\":0.0},\"calibrations\":[], \"cameraLEDs\":[]}"; - var writer = new FileWriter("test.json"); + File tempFile = File.createTempFile("test", ".json"); + tempFile.deleteOnExit(); + var writer = new FileWriter(tempFile); writer.write(str); writer.flush(); writer.close(); Assertions.assertDoesNotThrow( - () -> JacksonUtils.deserialize(Path.of("test.json"), CameraConfiguration.class)); + () -> JacksonUtils.deserialize(tempFile.toPath(), CameraConfiguration.class)); - new File("test.json").delete(); + tempFile.delete(); + } + + @Test + public void testJacksonHandlesOldTargetEnum() throws IOException { + var str = "[ \"AprilTagPipelineSettings\", {\n \"targetModel\" : \"k6in_16h5\"\n} ]\n"; + + File tempFile = File.createTempFile("test", ".json"); + tempFile.deleteOnExit(); + var writer = new FileWriter(tempFile); + writer.write(str); + writer.flush(); + writer.close(); + + AprilTagPipelineSettings settings = + (AprilTagPipelineSettings) + JacksonUtils.deserialize(tempFile.toPath(), CVPipelineSettings.class); + Assertions.assertEquals(TargetModel.kAprilTag6in_16h5, settings.targetModel); + + tempFile.delete(); } } From 796b8e73d56b0d3caa94abcc2384275ff797ba6d Mon Sep 17 00:00:00 2001 From: Craig Schardt Date: Mon, 18 Dec 2023 11:59:14 -0600 Subject: [PATCH 20/20] Fix Logger - ConfigManager circular reference (#1054) --- .../common/configuration/ConfigManager.java | 9 ++- .../common/configuration/PathManager.java | 75 +++++++++++++++++++ .../configuration/SqlConfigProvider.java | 2 +- .../photonvision/common/logging/Logger.java | 10 +-- 4 files changed, 86 insertions(+), 10 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java index 1046c3d6c6..e162f6aa16 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java @@ -62,12 +62,13 @@ enum ConfigSaveStrategy { public static ConfigManager getInstance() { if (INSTANCE == null) { + Path rootFolder = PathManager.getInstance().getRootFolder(); switch (m_saveStrat) { case SQL: - INSTANCE = new ConfigManager(getRootFolder(), new SqlConfigProvider(getRootFolder())); + INSTANCE = new ConfigManager(rootFolder, new SqlConfigProvider(rootFolder)); break; case LEGACY: - INSTANCE = new ConfigManager(getRootFolder(), new LegacyConfigProvider(getRootFolder())); + INSTANCE = new ConfigManager(rootFolder, new LegacyConfigProvider(rootFolder)); break; case ATOMIC_ZIP: // not yet done, fall through @@ -78,7 +79,7 @@ public static ConfigManager getInstance() { return INSTANCE; } - private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General); + private static final Logger logger = new Logger(ConfigManager.class, LogGroup.Config); private void translateLegacyIfPresent(Path folderPath) { if (!(m_provider instanceof SqlConfigProvider)) { @@ -167,7 +168,7 @@ public PhotonConfiguration getConfig() { } private static Path getRootFolder() { - return Path.of("photonvision_config"); + return PathManager.getInstance().getRootFolder(); } ConfigManager(Path configDirectory, ConfigProvider provider) { diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java new file mode 100644 index 0000000000..0a57d13d03 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java @@ -0,0 +1,75 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.common.configuration; + +import java.io.File; +import java.nio.file.Path; +import java.text.DateFormat; +import java.text.ParseException; +import java.text.SimpleDateFormat; +import java.time.LocalDateTime; +import java.time.format.DateTimeFormatter; +import java.time.temporal.TemporalAccessor; +import java.util.Date; + +public class PathManager { + private static PathManager INSTANCE; + + final File configDirectoryFile; + + public static PathManager getInstance() { + if (INSTANCE == null) { + INSTANCE = new PathManager(); + } + return INSTANCE; + } + + private PathManager() { + this.configDirectoryFile = new File(getRootFolder().toUri()); + } + + public Path getRootFolder() { + return Path.of("photonvision_config"); + } + + public Path getLogsDir() { + return Path.of(configDirectoryFile.toString(), "logs"); + } + + public static final String LOG_PREFIX = "photonvision-"; + public static final String LOG_EXT = ".log"; + public static final String LOG_DATE_TIME_FORMAT = "yyyy-M-d_hh-mm-ss"; + + public String taToLogFname(TemporalAccessor date) { + var dateString = DateTimeFormatter.ofPattern(LOG_DATE_TIME_FORMAT).format(date); + return LOG_PREFIX + dateString + LOG_EXT; + } + + public Path getLogPath() { + var logFile = Path.of(this.getLogsDir().toString(), taToLogFname(LocalDateTime.now())).toFile(); + if (!logFile.getParentFile().exists()) logFile.getParentFile().mkdirs(); + return logFile.toPath(); + } + + public Date logFnameToDate(String fname) throws ParseException { + // Strip away known unneeded portions of the log file name + fname = fname.replace(LOG_PREFIX, "").replace(LOG_EXT, ""); + DateFormat format = new SimpleDateFormat(LOG_DATE_TIME_FORMAT); + return format.parse(fname); + } +} diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 3f22e69522..883b3607d8 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -45,7 +45,7 @@ *

Global has one row per global config file (like hardware settings and network settings) */ public class SqlConfigProvider extends ConfigProvider { - private final Logger logger = new Logger(SqlConfigProvider.class, LogGroup.Config); + private static final Logger logger = new Logger(SqlConfigProvider.class, LogGroup.Config); static class TableKeys { static final String CAM_UNIQUE_NAME = "unique_name"; diff --git a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java index b06fea13fa..0e2ef96ef5 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java @@ -24,7 +24,8 @@ import java.util.*; import java.util.function.Supplier; import org.apache.commons.lang3.tuple.Pair; -import org.photonvision.common.configuration.ConfigManager; +// import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.configuration.PathManager; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.util.TimedTaskManager; @@ -103,8 +104,8 @@ public static String format( static { currentAppenders.add(new ConsoleLogAppender()); currentAppenders.add(uiLogAppender); - addFileAppender(ConfigManager.getInstance().getLogPath()); - cleanLogs(ConfigManager.getInstance().getLogsDir()); + addFileAppender(PathManager.getInstance().getLogPath()); + cleanLogs(PathManager.getInstance().getLogsDir()); } @SuppressWarnings("ResultOfMethodCallIgnored") @@ -133,8 +134,7 @@ public static void cleanLogs(Path folderToClean) { logFileList.removeIf( (File arg0) -> { try { - logFileStartDateMap.put( - arg0, ConfigManager.getInstance().logFnameToDate(arg0.getName())); + logFileStartDateMap.put(arg0, PathManager.getInstance().logFnameToDate(arg0.getName())); return false; } catch (ParseException e) { return true;