From f3193f4154dcc49d97353ccebeeb6f67f3416810 Mon Sep 17 00:00:00 2001 From: Matthew Morley Date: Fri, 28 Jul 2023 14:05:04 -0700 Subject: [PATCH] Fix most of the merge conflicts --- .../configuration/LegacyConfigProvider.java | 38 +++++++- .../configuration/SqlConfigProvider.java | 26 +++++- .../photonvision/estimation/PNPResults.java | 93 ------------------- .../estimation/VisionEstimation.java | 2 +- .../simulation/PhotonCameraSim.java | 2 +- .../photonvision/targeting/PNPResults.java | 59 ++++++++---- .../targeting/PhotonPipelineResult.java | 25 ++--- 7 files changed, 113 insertions(+), 132 deletions(-) delete mode 100644 photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java index 7051ab7ef9..f3576e5a9a 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java @@ -18,6 +18,8 @@ package org.photonvision.common.configuration; import com.fasterxml.jackson.core.JsonProcessingException; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import java.io.File; import java.io.IOException; import java.nio.file.Files; @@ -45,12 +47,14 @@ class LegacyConfigProvider extends ConfigProvider { public static final String HW_CFG_FNAME = "hardwareConfig.json"; public static final String HW_SET_FNAME = "hardwareSettings.json"; public static final String NET_SET_FNAME = "networkSettings.json"; + public static final String ATFL_SET_FNAME = "apriltagFieldLayout.json"; private PhotonConfiguration config; private final File hardwareConfigFile; private final File hardwareSettingsFile; private final File networkConfigFile; private final File camerasFolder; + private final File apriltagFieldLayoutFile; final File configDirectoryFile; @@ -87,6 +91,8 @@ protected LegacyConfigProvider(Path configDirectoryFile) { new File(Path.of(configDirectoryFile.toString(), HW_SET_FNAME).toUri()); this.networkConfigFile = new File(Path.of(configDirectoryFile.toString(), NET_SET_FNAME).toUri()); + this.apriltagFieldLayoutFile = + new File(Path.of(configDirectoryFile.toString(), ATFL_SET_FNAME).toUri()); this.camerasFolder = new File(Path.of(configDirectoryFile.toString(), "cameras").toUri()); settingsSaveThread = new Thread(this::saveAndWriteTask); @@ -117,6 +123,7 @@ public void load() { HardwareConfig hardwareConfig; HardwareSettings hardwareSettings; NetworkConfig networkConfig; + AprilTagFieldLayout atfl = null; if (hardwareConfigFile.exists()) { try { @@ -176,11 +183,40 @@ public void load() { } } + if (apriltagFieldLayoutFile.exists()) { + try { + atfl = + JacksonUtils.deserialize(apriltagFieldLayoutFile.toPath(), AprilTagFieldLayout.class); + if (atfl == null) { + logger.error("Could not deserialize apriltag field layout!"); + } + } catch (IOException e) { + logger.error("Could not deserialize apriltag field layout!"); + atfl = null; // not required, nice to be explicit + } + logger.info("Apriltag layout file not saved!"); + atfl = null; // not required, nice to be explicit + } + if (atfl == null) { + logger.info("Loading default apriltags for 2023 field..."); + try { + atfl = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + } catch (IOException e) { + logger.error("Error loading WPILib field", e); + atfl = null; + } + if (atfl == null) { + // what do we even do here lmao -- wpilib should always work + logger.error("Field layout is *still* null??????"); + atfl = new AprilTagFieldLayout(List.of(), 1, 1); + } + } + HashMap cameraConfigurations = loadCameraConfigs(); this.config = new PhotonConfiguration( - hardwareConfig, hardwareSettings, networkConfig, cameraConfigurations); + hardwareConfig, hardwareSettings, networkConfig, atfl, cameraConfigurations); } @Override 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 cad4325c4b..53e31e41f0 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 @@ -17,6 +17,8 @@ package org.photonvision.common.configuration; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import java.io.File; import java.io.IOException; import java.nio.file.Files; @@ -57,6 +59,7 @@ static class TableKeys { static final String NETWORK_CONFIG = "networkConfig"; static final String HARDWARE_CONFIG = "hardwareConfig"; static final String HARDWARE_SETTINGS = "hardwareSettings"; + static final String ATFL_CONFIG_FILE = "apriltagFieldLayout"; } private static final String dbName = "photon.sqlite"; @@ -206,6 +209,7 @@ public void load() { HardwareConfig hardwareConfig; HardwareSettings hardwareSettings; NetworkConfig networkConfig; + AprilTagFieldLayout atfl; try { hardwareConfig = @@ -234,6 +238,25 @@ public void load() { networkConfig = new NetworkConfig(); } + try { + atfl = + JacksonUtils.deserialize( + getOneConfigFile(conn, TableKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class); + } catch (IOException e) { + logger.error("Could not deserialize network config! Loading defaults"); + try { + atfl = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + } catch (IOException e2) { + logger.error("Error loading WPILib field", e); + atfl = null; + } + if (atfl == null) { + // what do we even do here lmao -- wpilib should always work + logger.error("Field layout is *still* null??????"); + atfl = new AprilTagFieldLayout(List.of(), 1, 1); + } + } + var cams = loadCameraConfigs(conn); try { @@ -242,7 +265,8 @@ public void load() { logger.error("SQL Err closing connection while loading: ", e); } - this.config = new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, cams); + this.config = + new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, atfl, cams); } } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java deleted file mode 100644 index 89adf94bcf..0000000000 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ /dev/null @@ -1,93 +0,0 @@ -/* - * 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. - */ - -package org.photonvision.estimation; - -import edu.wpi.first.math.geometry.Transform3d; - -/** - * The best estimated transformation from solvePnP, and possibly an alternate transformation - * depending on the solvePNP method. If an alternate solution is present, the ambiguity value - * represents the ratio of reprojection error in the best solution to the alternate (best / - * alternate). - * - *

Note that the coordinate frame of these transforms depends on the implementing solvePnP - * method. - */ -public class PNPResults { - /** - * If this result is valid. A false value indicates there was an error in estimation, and this - * result should not be used. - */ - public final boolean isPresent; - - /** - * The best-fit transform. The coordinate frame of this transform depends on the method which gave - * this result. - */ - public final Transform3d best; - - /** Reprojection error of the best solution, in pixels */ - public final double bestReprojErr; - - /** - * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal - * to the best solution. - */ - public final Transform3d alt; - - /** If no alternate solution is found, this is bestReprojErr */ - public final double altReprojErr; - - /** If no alternate solution is found, this is 0 */ - public final double ambiguity; - - /** An empty (invalid) result. */ - public PNPResults() { - this.isPresent = false; - this.best = new Transform3d(); - this.alt = new Transform3d(); - this.ambiguity = 0; - this.bestReprojErr = 0; - this.altReprojErr = 0; - } - - public PNPResults(Transform3d best, double bestReprojErr) { - this(best, best, 0, bestReprojErr, bestReprojErr); - } - - public PNPResults( - Transform3d best, - Transform3d alt, - double ambiguity, - double bestReprojErr, - double altReprojErr) { - this.isPresent = true; - this.best = best; - this.alt = alt; - this.ambiguity = ambiguity; - this.bestReprojErr = bestReprojErr; - this.altReprojErr = altReprojErr; - } -} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 1acf65743a..caeda4fcda 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -33,9 +33,9 @@ import edu.wpi.first.math.numbers.*; import java.util.ArrayList; import java.util.List; -import org.photonvision.targeting.PNPResults; import java.util.Objects; import java.util.stream.Collectors; +import org.photonvision.targeting.PNPResults; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 81a20d0d36..0c1fd875eb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -48,7 +48,7 @@ import org.photonvision.common.networktables.NTTopicSet; import org.photonvision.estimation.CameraTargetRelation; import org.photonvision.estimation.OpenCVHelp; -import org.photonvision.estimation.PNPResults; +import org.photonvision.targeting.PNPResults; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index 89adf94bcf..da42406711 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -1,30 +1,25 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * 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: + * 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. * - * 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. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ -package org.photonvision.estimation; +package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.utils.PacketUtils; /** * The best estimated transformation from solvePnP, and possibly an alternate transformation @@ -90,4 +85,30 @@ public PNPResults( this.bestReprojErr = bestReprojErr; this.altReprojErr = altReprojErr; } + + public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); + + public static PNPResults createFromPacket(Packet packet) { + var present = packet.decodeBoolean(); + var best = PacketUtils.decodeTransform(packet); + var alt = PacketUtils.decodeTransform(packet); + var bestEr = packet.decodeDouble(); + var altEr = packet.decodeDouble(); + var ambiguity = packet.decodeDouble(); + if (present) { + return new PNPResults(best, alt, ambiguity, bestEr, altEr); + } else { + return new PNPResults(); + } + } + + public Packet populatePacket(Packet packet) { + packet.encode(isPresent); + PacketUtils.encodeTransform(packet, best); + PacketUtils.encodeTransform(packet, alt); + packet.encode(bestReprojErr); + packet.encode(altReprojErr); + packet.encode(ambiguity); + return packet; + } } 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 7237beba7f..e7aee05c9b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -209,27 +209,20 @@ public int hashCode() { @Override public boolean equals(Object obj) { - if (this == obj) - return true; - if (obj == null) - return false; - if (getClass() != obj.getClass()) - return false; + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; PhotonPipelineResult other = (PhotonPipelineResult) obj; if (targets == null) { - if (other.targets != null) - return false; - } else if (!targets.equals(other.targets)) - return false; + if (other.targets != null) return false; + } else if (!targets.equals(other.targets)) return false; if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) return false; - if (Double.doubleToLongBits(timestampSeconds) != Double.doubleToLongBits(other.timestampSeconds)) - return false; + if (Double.doubleToLongBits(timestampSeconds) + != Double.doubleToLongBits(other.timestampSeconds)) return false; if (multiTagResult == null) { - if (other.multiTagResult != null) - return false; - } else if (!multiTagResult.equals(other.multiTagResult)) - return false; + if (other.multiTagResult != null) return false; + } else if (!multiTagResult.equals(other.multiTagResult)) return false; return true; } }