diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue index c242344a92..c8831e944b 100644 --- a/photon-client/src/components/dashboard/tabs/PnPTab.vue +++ b/photon-client/src/components/dashboard/tabs/PnPTab.vue @@ -26,11 +26,8 @@ const interactiveCols = computed( { name: '2020 High Goal Outer', value: TargetModel.InfiniteRechargeHighGoalOuter }, { name: '2020 Power Cell (7in)', value: TargetModel.CircularPowerCell7in }, { name: '2022 Cargo Ball (9.5in)', value: TargetModel.RapidReactCircularCargoBall }, - { name: '200mm AprilTag', value: TargetModel.Apriltag_200mm }, - { name: '6in (16h5) Aruco', value: TargetModel.Aruco6in_16h5 }, - { name: '6in (16h5) AprilTag', value: TargetModel.Apriltag6in_16h5 }, - { name: '6.5in (36h11) Aruco', value: TargetModel.Aruco6p5in_36h11 }, - { name: '6.5in (36h11) AprilTag', value: TargetModel.Apriltag6p5in_36h11 } + { name: '2023 AprilTag 6in (16h5)', value: TargetModel.AprilTag6in_16h5 }, + { name: '2024 AprilTag 6.5in (36h11)', value: TargetModel.AprilTag6p5in_36h11 } ]" :select-cols="interactiveCols" @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ targetModel: value }, false)" diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index 08d231b732..860c87e757 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -26,11 +26,8 @@ export enum TargetModel { InfiniteRechargeHighGoalOuter = 2, CircularPowerCell7in = 3, RapidReactCircularCargoBall = 4, - Apriltag_200mm = 5, - Aruco6in_16h5 = 6, - Apriltag6in_16h5 = 7, - Aruco6p5in_36h11 = 8, - Apriltag6p5in_36h11 = 9 + AprilTag6in_16h5 = 5, + AprilTag6p5in_36h11 = 6 } export interface PipelineSettings { @@ -225,7 +222,7 @@ export type ConfigurableAprilTagPipelineSettings = Partial< export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = { ...DefaultPipelineSettings, cameraGain: 75, - targetModel: TargetModel.Apriltag6in_16h5, + targetModel: TargetModel.AprilTag6in_16h5, ledMode: false, outputShowMultipleTargets: true, cameraExposure: 20, @@ -268,7 +265,7 @@ export type ConfigurableArucoPipelineSettings = Partial MAX_FRAME_PERIOD_NS) { + lastFrameTimeNs = now; + cvSource.putFrame(image.getMat()); + } // Make sure our disabled framerate limiting doesn't get confused isDisabled = false; - lastEmptyTime = 0; + lastEmptyTimeNs = 0; } } @@ -182,9 +192,10 @@ public void disabledTick() { isDisabled = true; } - if (System.currentTimeMillis() - lastEmptyTime > 1000.0 / EMPTY_FRAMERATE) { + long now = MathUtils.wpiNanoTime(); + if (now - lastEmptyTimeNs > EMPTY_FRAME_PERIOD_NS) { + lastEmptyTimeNs = now; cvSource.putFrame(EMPTY_MAT); - lastEmptyTime = System.currentTimeMillis(); } } @@ -233,6 +244,7 @@ private static String pixelFormatToString(VideoMode.PixelFormat pixelFormat) { } } + @Override public void close() { table.getEntry("connected").setBoolean(false); mjpegServer.close(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java index 5ce9f124cc..313d8a19e0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java @@ -22,6 +22,7 @@ import java.util.List; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; @@ -38,7 +39,8 @@ public class MultiTargetPNPPipe @Override protected MultiTargetPNPResults process(List targetList) { - if (params.cameraCoefficients == null + if (params == null + || params.cameraCoefficients == null || params.cameraCoefficients.getCameraIntrinsicsMat() == null || params.cameraCoefficients.getDistCoeffsMat() == null) { if (!hasWarned) { @@ -70,7 +72,8 @@ private MultiTargetPNPResults calculateCameraInField(List targetL params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(), params.cameraCoefficients.distCoeffs.getAsWpilibMat(), TrackedTarget.simpleFromTrackedTargets(targetList), - params.atfl); + params.atfl, + params.targetModel); return new MultiTargetPNPResults(estimatedPose, tagIDsUsed); } @@ -78,11 +81,15 @@ private MultiTargetPNPResults calculateCameraInField(List targetL public static class MultiTargetPNPPipeParams { private final CameraCalibrationCoefficients cameraCoefficients; private final AprilTagFieldLayout atfl; + private final TargetModel targetModel; public MultiTargetPNPPipeParams( - CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) { + CameraCalibrationCoefficients cameraCoefficients, + AprilTagFieldLayout atfl, + TargetModel targetModel) { this.cameraCoefficients = cameraCoefficients; this.atfl = atfl; + this.targetModel = targetModel; } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 4a17673c94..1eb040616a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -30,6 +30,7 @@ import java.util.List; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; +import org.photonvision.estimation.TargetModel; import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.frame.Frame; @@ -71,13 +72,13 @@ protected void setPipeParamsImpl() { settings.threads = Math.max(1, settings.threads); // for now, hard code tag width based on enum value - double tagWidth; + // 2023/other: best guess is 6in + double tagWidth = Units.inchesToMeters(6); + TargetModel tagModel = TargetModel.kAprilTag16h5; if (settings.tagFamily == AprilTagFamily.kTag36h11) { - // 2024 tag, guess 6.5in + // 2024 tag, 6.5in tagWidth = Units.inchesToMeters(6.5); - } else { - // 2023/other: best guess is 6in - tagWidth = Units.inchesToMeters(6); + tagModel = TargetModel.kAprilTag36h11; } var config = new AprilTagDetector.Config(); @@ -104,7 +105,7 @@ protected void setPipeParamsImpl() { // TODO global state ew var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); multiTagPNPPipe.setParams( - new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl)); + new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel)); } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java index 6db514d005..31da770127 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java @@ -41,7 +41,7 @@ public AprilTagPipelineSettings() { super(); pipelineType = PipelineType.AprilTag; outputShowMultipleTargets = true; - targetModel = TargetModel.k6in_16h5; + targetModel = TargetModel.kAprilTag6in_16h5; cameraExposure = 20; cameraAutoExposure = false; ledMode = false; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index 4d80029cbd..c463cb14e4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -46,6 +46,7 @@ import org.opencv.objdetect.Objdetect; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; +import org.photonvision.estimation.TargetModel; import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.Frame; @@ -79,9 +80,16 @@ protected void setPipeParamsImpl() { var params = new ArucoDetectionPipeParams(); // sanitize and record settings + // for now, hard code tag width based on enum value + // 2023/other: best guess is 6in + double tagWidth = Units.inchesToMeters(6); + TargetModel tagModel = TargetModel.kAprilTag16h5; switch (settings.tagFamily) { case kTag36h11: + // 2024 tag, 6.5in params.tagFamily = Objdetect.DICT_APRILTAG_36h11; + tagWidth = Units.inchesToMeters(6.5); + tagModel = TargetModel.kAprilTag36h11; break; case kTag25h9: params.tagFamily = Objdetect.DICT_APRILTAG_25h9; @@ -113,14 +121,13 @@ protected void setPipeParamsImpl() { var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat(); if (cameraMatrix != null && cameraMatrix.rows() > 0) { var estimatorParams = - new ArucoPoseEstimatorPipeParams( - frameStaticProperties.cameraCalibration, Units.inchesToMeters(6)); + new ArucoPoseEstimatorPipeParams(frameStaticProperties.cameraCalibration, tagWidth); singleTagPoseEstimatorPipe.setParams(estimatorParams); // TODO global state ew var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); multiTagPNPPipe.setParams( - new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl)); + new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel)); } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java index 94eb963435..2690034db4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java @@ -46,7 +46,7 @@ public ArucoPipelineSettings() { super(); pipelineType = PipelineType.Aruco; outputShowMultipleTargets = true; - targetModel = TargetModel.k6in_16h5; + targetModel = TargetModel.kAprilTag6in_16h5; cameraExposure = -1; cameraAutoExposure = true; ledMode = false; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index 0474b6b562..238333ee6a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -107,7 +107,7 @@ protected void setPipeParams( new Draw3dArucoPipe.Draw3dArucoParams( settings.outputShouldDraw, frameStaticProperties.cameraCalibration, - TargetModel.k6in_16h5, + TargetModel.kAprilTag6in_16h5, settings.streamingFrameDivisor); draw3dArucoPipe.setParams(draw3dArucoParams); 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 75978739e7..54daf5d3e5 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 @@ -17,6 +17,7 @@ package org.photonvision.vision.processes; +import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.cscore.VideoException; import edu.wpi.first.math.util.Units; import io.javalin.websocket.WsContext; @@ -44,6 +45,7 @@ import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer; +import org.photonvision.vision.frame.consumer.MJPGFrameConsumer; import org.photonvision.vision.pipeline.AdvancedPipelineSettings; import org.photonvision.vision.pipeline.OutputStreamPipeline; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; @@ -51,8 +53,6 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; -import org.photonvision.vision.videoStream.SocketVideoStream; -import org.photonvision.vision.videoStream.SocketVideoStreamManager; /** * This is the God Class @@ -83,8 +83,8 @@ public class VisionModule { FileSaveFrameConsumer inputFrameSaver; FileSaveFrameConsumer outputFrameSaver; - SocketVideoStream inputVideoStreamer; - SocketVideoStream outputVideoStreamer; + MJPGFrameConsumer inputVideoStreamer; + MJPGFrameConsumer outputVideoStreamer; public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = @@ -169,11 +169,6 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, saveAndBroadcastAll(); } - private void destroyStreams() { - SocketVideoStreamManager.getInstance().removeStream(inputVideoStreamer); - SocketVideoStreamManager.getInstance().removeStream(outputVideoStreamer); - } - private void createStreams() { var camStreamIdx = visionSource.getSettables().getConfiguration().streamIndex; // If idx = 0, we want (1181, 1182) @@ -186,10 +181,13 @@ private void createStreams() { new FileSaveFrameConsumer( visionSource.getSettables().getConfiguration().nickname, "output"); - inputVideoStreamer = new SocketVideoStream(this.inputStreamPort); - outputVideoStreamer = new SocketVideoStream(this.outputStreamPort); - SocketVideoStreamManager.getInstance().addStream(inputVideoStreamer); - SocketVideoStreamManager.getInstance().addStream(outputVideoStreamer); + String camHostname = CameraServerJNI.getHostname(); + inputVideoStreamer = + new MJPGFrameConsumer( + camHostname + "_Port_" + inputStreamPort + "_Input_MJPEG_Server", inputStreamPort); + outputVideoStreamer = + new MJPGFrameConsumer( + camHostname + "_Port_" + outputStreamPort + "_Output_MJPEG_Server", outputStreamPort); } private void recreateStreamResultConsumers() { @@ -483,16 +481,6 @@ public void setCameraNickname(String newName) { inputFrameSaver.updateCameraNickname(newName); outputFrameSaver.updateCameraNickname(newName); - // Rename streams - streamResultConsumers.clear(); - - // Teardown and recreate streams - destroyStreams(); - createStreams(); - - // Rebuild streamers - recreateStreamResultConsumers(); - // Push new data to the UI saveAndBroadcastAll(); } 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 3e8a967ba2..1cd3579382 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 @@ -107,30 +107,18 @@ public enum TargetModel implements Releasable { -Units.inchesToMeters(9.5) / 2, -Units.inchesToMeters(9.5) / 2)), 0), - k200mmAprilTag( // Corners of the tag's inner black square (excluding white border) - List.of( - new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0), - new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0), - new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0), - new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)), - Units.inchesToMeters(3.25 * 2)), - k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + // 2023 AprilTag, with 6 inch marker width (inner black square). + kAprilTag6in_16h5( + // Corners of the tag's inner black square (excluding white border) List.of( new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0), new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0), new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), Units.inchesToMeters(3 * 2)), - // 2024 FRC tag. 6.5in inner tag, 8.125 overall - kAruco6p5in_36h11( - List.of( - new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)), - Units.inchesToMeters(6.5)), - k6p5in_36h11( + // 2024 AprilTag, with 6.5 inch marker width (inner black square). + kAprilTag6p5in_36h11( + // Corners of the tag's inner black square (excluding white border) List.of( new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), diff --git a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java deleted file mode 100644 index dae270e9f6..0000000000 --- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java +++ /dev/null @@ -1,117 +0,0 @@ -/* - * 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.vision.videoStream; - -import edu.wpi.first.cscore.CameraServerJNI; -import java.nio.ByteBuffer; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Consumer; -import org.opencv.core.MatOfByte; -import org.opencv.core.MatOfInt; -import org.opencv.imgcodecs.Imgcodecs; -import org.photonvision.common.util.math.MathUtils; -import org.photonvision.vision.frame.consumer.MJPGFrameConsumer; -import org.photonvision.vision.opencv.CVMat; - -public class SocketVideoStream implements Consumer { - int portID = 0; // Align with cscore's port for unique identification of stream - MatOfByte jpegBytes = null; - - // Gets set to true when another class reads out valid jpeg bytes at least once - // Set back to false when another frame is freshly converted - // Should eliminate synchronization issues of differing rates of putting frames in - // and taking them back out - boolean frameWasConsumed = false; - - // Synclock around manipulating the jpeg bytes from multiple threads - Lock jpegBytesLock = new ReentrantLock(); - private int userCount = 0; - - // FPS-limited MJPEG sender - private final double FPS_MAX = 30.0; - private final long minFramePeriodNanos = Math.round(1000000000.0 / FPS_MAX); - private long nextFrameSendTime = MathUtils.wpiNanoTime() + minFramePeriodNanos; - MJPGFrameConsumer oldSchoolServer; - - public SocketVideoStream(int portID) { - this.portID = portID; - oldSchoolServer = - new MJPGFrameConsumer( - CameraServerJNI.getHostname() + "_Port_" + portID + "_MJPEG_Server", portID); - } - - @Override - public void accept(CVMat image) { - if (userCount > 0) { - if (jpegBytesLock - .tryLock()) { // we assume frames are coming in frequently. Just skip this frame if we're - // locked doing something else. - try { - // Does a single-shot frame receive and convert to JPEG for efficiency - // Will not capture/convert again until convertNextFrame() is called - if (image != null && !image.getMat().empty() && jpegBytes == null) { - frameWasConsumed = false; - jpegBytes = new MatOfByte(); - Imgcodecs.imencode( - ".jpg", - image.getMat(), - jpegBytes, - new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 75)); - } - } finally { - jpegBytesLock.unlock(); - } - } - } - - // Send the frame in an FPS-limited fashion - var now = MathUtils.wpiNanoTime(); - if (now > nextFrameSendTime) { - oldSchoolServer.accept(image); - nextFrameSendTime = now + minFramePeriodNanos; - } - } - - public ByteBuffer getJPEGByteBuffer() { - ByteBuffer sendStr = null; - jpegBytesLock.lock(); - if (jpegBytes != null) { - sendStr = ByteBuffer.wrap(jpegBytes.toArray()); - } - jpegBytesLock.unlock(); - return sendStr; - } - - public void convertNextFrame() { - jpegBytesLock.lock(); - if (jpegBytes != null) { - jpegBytes.release(); - jpegBytes = null; - } - jpegBytesLock.unlock(); - } - - public void addUser() { - userCount++; - } - - public void removeUser() { - userCount--; - } -} diff --git a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java deleted file mode 100644 index ceb84feab6..0000000000 --- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java +++ /dev/null @@ -1,101 +0,0 @@ -/* - * 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.vision.videoStream; - -import io.javalin.websocket.WsContext; -import java.nio.ByteBuffer; -import java.util.Hashtable; -import java.util.Map; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; - -public class SocketVideoStreamManager { - private static final int NO_STREAM_PORT = -1; - - private final Logger logger = new Logger(SocketVideoStreamManager.class, LogGroup.Camera); - - private final Map streams = new Hashtable<>(); - private final Map userSubscriptions = new Hashtable<>(); - - private static class ThreadSafeSingleton { - private static final SocketVideoStreamManager INSTANCE = new SocketVideoStreamManager(); - } - - public static SocketVideoStreamManager getInstance() { - return ThreadSafeSingleton.INSTANCE; - } - - private SocketVideoStreamManager() {} - - // Register a new available camera stream - public void addStream(SocketVideoStream newStream) { - streams.put(newStream.portID, newStream); - logger.debug("Added new stream for port " + newStream.portID); - } - - // Remove a previously-added camera stream, and unsubscribe all users - public void removeStream(SocketVideoStream oldStream) { - streams.remove(oldStream.portID); - logger.debug("Removed stream for port " + oldStream.portID); - } - - // Indicate a user would like to subscribe to a camera stream and get frames from it periodically - public void addSubscription(WsContext user, int streamPortID) { - var stream = streams.get(streamPortID); - if (stream != null) { - userSubscriptions.put(user, streamPortID); - stream.addUser(); - } else { - logger.error("User attempted to subscribe to non-existent port " + streamPortID); - } - } - - // Indicate a user would like to stop receiving one camera stream - public void removeSubscription(WsContext user) { - var port = userSubscriptions.get(user); - if (port != null && port != NO_STREAM_PORT) { - var stream = streams.get(port); - userSubscriptions.put(user, NO_STREAM_PORT); - if (stream != null) { - stream.removeUser(); - } - } else { - logger.error( - "User attempted to unsubscribe, but had not yet previously subscribed successfully."); - } - } - - // For a given user, return the jpeg bytes (or null) for the most recent frame - public ByteBuffer getSendFrame(WsContext user) { - var port = userSubscriptions.get(user); - if (port != null && port != NO_STREAM_PORT) { - var stream = streams.get(port); - return stream.getJPEGByteBuffer(); - } else { - return null; - } - } - - // Causes all streams to "re-trigger" and receive and convert their next mjpeg frame - // Only invoke this after all returned jpeg Strings have been used. - public void allStreamConvertNextFrame() { - for (SocketVideoStream stream : streams.values()) { - stream.convertNextFrame(); - } - } -} diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java index a970b1e3f7..24bffd6d55 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java @@ -47,7 +47,7 @@ public void testApriltagFacingCamera() { pipeline.getSettings().solvePNPEnabled = true; pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; pipeline.getSettings().cornerDetectionUseConvexHulls = true; - pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag; + pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11; pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11; var frameProvider = @@ -112,7 +112,7 @@ public void testApriltagDistorted() { pipeline.getSettings().solvePNPEnabled = true; pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; pipeline.getSettings().cornerDetectionUseConvexHulls = true; - pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag; + pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11; pipeline.getSettings().tagFamily = AprilTagFamily.kTag16h5; var frameProvider = diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java index 105858e85b..8a7a4af464 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java @@ -47,7 +47,7 @@ public void testApriltagFacingCamera() { pipeline.getSettings().solvePNPEnabled = true; pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; pipeline.getSettings().cornerDetectionUseConvexHulls = true; - pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag; + pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11; pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11; var frameProvider = diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index ed29a9c265..1c73f2cab2 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -41,6 +41,7 @@ import java.util.List; import java.util.Optional; import java.util.Set; +import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -82,6 +83,7 @@ public enum PoseStrategy { } private AprilTagFieldLayout fieldTags; + private TargetModel tagModel = TargetModel.kAprilTag16h5; private PoseStrategy primaryStrategy; private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; private final PhotonCamera camera; @@ -155,6 +157,24 @@ public void setFieldTags(AprilTagFieldLayout fieldTags) { this.fieldTags = fieldTags; } + /** + * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. + * + *

By default, this is {@link TargetModel#kAprilTag16h5}. + */ + public TargetModel getTagModel() { + return tagModel; + } + + /** + * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. + * + * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. + */ + public void setTagModel(TargetModel tagModel) { + this.tagModel = tagModel; + } + /** * Get the Position Estimation Strategy being used by the Position Estimator. * @@ -419,7 +439,7 @@ private Optional multiTagOnRioStrategy( var pnpResults = VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags); + cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); // try fallback strategy if solvePNP fails for some reason if (!pnpResults.isPresent) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); 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 b2c7317c32..4880adaa85 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -531,7 +531,11 @@ public PhotonPipelineResult process( visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList()); var pnpResults = VisionEstimation.estimateCamPosePNP( - prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout); + prop.getIntrinsics(), + prop.getDistCoeffs(), + detectableTgts, + tagLayout, + TargetModel.kAprilTag16h5); multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index ed0207096e..573ad77e91 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -257,7 +257,7 @@ public void addAprilTags(AprilTagFieldLayout tagLayout) { "apriltag", new VisionTargetSim( tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, tag.ID)); } } diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 3cab3d496e..52b61f5739 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -137,7 +137,7 @@ public void testRotConvert() { public void testProjection() { var target = new VisionTargetSim( - new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0); + new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kAprilTag16h5, 0); var cameraPose = new Pose3d(0, 0, 0, new Rotation3d()); var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose); var imagePoints = @@ -193,7 +193,7 @@ public void testSolvePNP_SQUARE() { // square AprilTag target var target = new VisionTargetSim( - new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0); + new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kAprilTag16h5, 0); var cameraPose = new Pose3d(0, 0, 0, new Rotation3d()); var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose); // target relative to camera diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index d9ed86d8ed..c668621fdb 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -415,67 +415,67 @@ public void testMultipleTargets() { new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 1)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 2)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 3)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 4)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 5)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 6)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 7)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 8)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 9)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 10)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 11)); var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); @@ -506,7 +506,7 @@ public void testPoseEstimation() { Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0)); + new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0)); visionSysSim.update(robotPose); var results = @@ -514,7 +514,8 @@ public void testPoseEstimation() { camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), - layout); + layout, + TargetModel.kAprilTag16h5); Pose3d pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); @@ -522,9 +523,9 @@ public void testPoseEstimation() { assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(1).pose, TargetModel.kTag16h5, 1)); + new VisionTargetSim(tagList.get(1).pose, TargetModel.kAprilTag16h5, 1)); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2)); + new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2)); visionSysSim.update(robotPose); results = @@ -532,7 +533,8 @@ public void testPoseEstimation() { camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), - layout); + layout, + TargetModel.kAprilTag16h5); pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index db8787208d..f01e431d30 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -251,7 +251,7 @@ private static void addTestModeSources() { var pipeline2023 = new AprilTagPipelineSettings(); var path_split = Path.of(camConf2023.path).getFileName().toString(); pipeline2023.pipelineNickname = path_split.replace(".png", ""); - pipeline2023.targetModel = TargetModel.k6in_16h5; + pipeline2023.targetModel = TargetModel.kAprilTag6in_16h5; pipeline2023.inputShouldShow = true; pipeline2023.solvePNPEnabled = true; diff --git a/photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java b/photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java deleted file mode 100644 index d93f9a9e44..0000000000 --- a/photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java +++ /dev/null @@ -1,143 +0,0 @@ -/* - * 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.server; - -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.ObjectMapper; -import io.javalin.websocket.WsBinaryMessageContext; -import io.javalin.websocket.WsCloseContext; -import io.javalin.websocket.WsConnectContext; -import io.javalin.websocket.WsContext; -import io.javalin.websocket.WsMessageContext; -import java.net.InetSocketAddress; -import java.time.Duration; -import java.util.HashMap; -import java.util.List; -import java.util.concurrent.CopyOnWriteArrayList; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; -import org.photonvision.vision.videoStream.SocketVideoStreamManager; - -public class CameraSocketHandler { - private final Logger logger = new Logger(CameraSocketHandler.class, LogGroup.WebServer); - private final List users = new CopyOnWriteArrayList<>(); - private final SocketVideoStreamManager svsManager = SocketVideoStreamManager.getInstance(); - - private Thread cameraBroadcastThread; - - public static class UIMap extends HashMap {} - - private static class ThreadSafeSingleton { - private static final CameraSocketHandler INSTANCE = new CameraSocketHandler(); - } - - public static CameraSocketHandler getInstance() { - return CameraSocketHandler.ThreadSafeSingleton.INSTANCE; - } - - private CameraSocketHandler() { - cameraBroadcastThread = new Thread(this::broadcastFramesTask); - cameraBroadcastThread.setPriority(Thread.MAX_PRIORITY - 3); // fairly high priority - cameraBroadcastThread.start(); - } - - public void onConnect(WsConnectContext context) { - context.session.setIdleTimeout( - Duration.ofMillis(Long.MAX_VALUE)); // TODO: determine better value - var remote = (InetSocketAddress) context.session.getRemoteAddress(); - var host = remote.getAddress().toString() + ":" + remote.getPort(); - logger.info("New camera websocket connection from " + host); - users.add(context); - } - - protected void onClose(WsCloseContext context) { - var remote = (InetSocketAddress) context.session.getRemoteAddress(); - var host = remote.getAddress().toString() + ":" + remote.getPort(); - var reason = context.reason() != null ? context.reason() : "Connection closed by client"; - logger.info("Closing camera websocket connection from " + host + " for reason: " + reason); - svsManager.removeSubscription(context); - users.remove(context); - } - - @SuppressWarnings({"unchecked"}) - public void onMessage(WsMessageContext context) { - var messageStr = context.message(); - ObjectMapper mapper = new ObjectMapper(); - try { - JsonNode actualObj = mapper.readTree(messageStr); - - try { - var entryCmd = actualObj.get("cmd").asText(); - var socketMessageType = CameraSocketMessageType.fromEntryKey(entryCmd); - - logger.trace(() -> "Got Camera WS message: [" + socketMessageType + "]"); - - if (socketMessageType == null) { - logger.warn("Got unknown socket message command: " + entryCmd); - } - - switch (socketMessageType) { - case CSMT_SUBSCRIBE: - { - int portId = actualObj.get("port").asInt(); - svsManager.addSubscription(context, portId); - break; - } - case CSMT_UNSUBSCRIBE: - { - svsManager.removeSubscription(context); - break; - } - } - } catch (Exception e) { - logger.error("Failed to parse message!", e); - } - - } catch (JsonProcessingException e) { - logger.warn("Could not parse message \"" + messageStr + "\""); - e.printStackTrace(); - return; - } - } - - @SuppressWarnings({"unchecked"}) - public void onBinaryMessage(WsBinaryMessageContext context) { - return; // ignoring binary messages for now - } - - private void broadcastFramesTask() { - // Background camera image broadcasting thread - while (!Thread.currentThread().isInterrupted()) { - svsManager.allStreamConvertNextFrame(); - - try { - Thread.sleep(1); - } catch (InterruptedException e) { - logger.error("Exception waiting for camera stream broadcast semaphore", e); - } - - for (var user : users) { - var sendBytes = svsManager.getSendFrame(user); - if (sendBytes != null) { - user.send(sendBytes); - } - } - } - } -} diff --git a/photon-server/src/main/java/org/photonvision/server/CameraSocketMessageType.java b/photon-server/src/main/java/org/photonvision/server/CameraSocketMessageType.java deleted file mode 100644 index 74841e2022..0000000000 --- a/photon-server/src/main/java/org/photonvision/server/CameraSocketMessageType.java +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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.server; - -import java.util.EnumSet; -import java.util.HashMap; -import java.util.Map; - -@SuppressWarnings("unused") -public enum CameraSocketMessageType { - CSMT_SUBSCRIBE("subscribe"), - CSMT_UNSUBSCRIBE("unsubscribe"); - - public final String entryKey; - - CameraSocketMessageType(String entryKey) { - this.entryKey = entryKey; - } - - private static final Map entryKeyToValueMap = new HashMap<>(); - - static { - for (var value : EnumSet.allOf(CameraSocketMessageType.class)) { - entryKeyToValueMap.put(value.entryKey, value); - } - } - - public static CameraSocketMessageType fromEntryKey(String entryKey) { - return entryKeyToValueMap.get(entryKey); - } -} diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java index 4026ab58fa..a5d2580ba2 100644 --- a/photon-server/src/main/java/org/photonvision/server/Server.java +++ b/photon-server/src/main/java/org/photonvision/server/Server.java @@ -79,17 +79,6 @@ public static void start(int port) { ws.onBinaryMessage(dsHandler::onBinaryMessage); }); - /*Web Socket Events for Camera Streaming */ - var camDsHandler = CameraSocketHandler.getInstance(); - app.ws( - "/websocket_cameras", - ws -> { - ws.onConnect(camDsHandler::onConnect); - ws.onClose(camDsHandler::onClose); - ws.onBinaryMessage(camDsHandler::onBinaryMessage); - ws.onMessage(camDsHandler::onMessage); - }); - /*API Events*/ // Settings app.post("/api/settings", RequestHandler::onSettingsImportRequest); diff --git a/photon-server/src/main/java/org/photonvision/server/UISettings.java b/photon-server/src/main/java/org/photonvision/server/UISettings.java deleted file mode 100644 index f4361c2670..0000000000 --- a/photon-server/src/main/java/org/photonvision/server/UISettings.java +++ /dev/null @@ -1,26 +0,0 @@ -/* - * 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.server; - -import org.photonvision.common.configuration.CameraConfiguration; -import org.photonvision.common.configuration.NetworkConfig; - -public class UISettings { - public NetworkConfig networkConfig; - public CameraConfiguration currentCameraConfiguration; -} diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index 4a4c22449d..f945d2c806 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -37,8 +37,10 @@ public class TargetModel { public final boolean isPlanar; public final boolean isSpherical; - public static final TargetModel kTag16h5 = + public static final TargetModel kAprilTag16h5 = new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + public static final TargetModel kAprilTag36h11 = + new TargetModel(Units.inchesToMeters(6.5), Units.inchesToMeters(6.5)); /** * Creates a rectangular, planar target model given the width and height. The model has four diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 387176d57b..9ad6e0fc4e 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -70,7 +70,8 @@ public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, - AprilTagFieldLayout tagLayout) { + AprilTagFieldLayout tagLayout, + TargetModel tagModel) { if (tagLayout == null || visTags == null || tagLayout.getTags().size() == 0 @@ -99,8 +100,7 @@ public static PNPResults estimateCamPosePNP( // single-tag pnp if (knownTags.size() == 1) { var camToTag = - OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points); + OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); if (!camToTag.isPresent) return new PNPResults(); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); @@ -118,7 +118,7 @@ public static PNPResults estimateCamPosePNP( // multi-tag pnp else { var objectTrls = new ArrayList(); - for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose)); + for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); if (!camToOrigin.isPresent) return new PNPResults(); return new PNPResults(