From 0898dfe2f7253fb526af3506f6fd907e81c28ebf Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 29 Oct 2023 23:02:16 -0400 Subject: [PATCH 1/6] Aruco/Multitag 36h11 support (#981) - Aruco pipeline now infers tag width from tag family like the AprilTag pipeline - Removes unused Aruco and 200mm AprilTag models - `VisionEstimation.estimateCamPosePNP()` now requires a target model instead of assuming 16h5 - Multitarget pipeline similarly infers target model of tag family now - `PhotonPoseEstimator` can have target model set for on-rio multitarget --------- Co-authored-by: amquake --- .../src/components/dashboard/tabs/PnPTab.vue | 7 ++-- photon-client/src/types/PipelineTypes.ts | 11 +++--- .../vision/pipe/impl/MultiTargetPNPPipe.java | 10 ++++-- .../vision/pipeline/AprilTagPipeline.java | 13 +++---- .../pipeline/AprilTagPipelineSettings.java | 2 +- .../vision/pipeline/ArucoPipeline.java | 13 +++++-- .../pipeline/ArucoPipelineSettings.java | 2 +- .../vision/pipeline/OutputStreamPipeline.java | 2 +- .../vision/target/TargetModel.java | 24 ++++--------- .../vision/pipeline/AprilTagTest.java | 4 +-- .../vision/pipeline/ArucoPipelineTest.java | 2 +- .../org/photonvision/PhotonPoseEstimator.java | 22 +++++++++++- .../simulation/PhotonCameraSim.java | 6 +++- .../simulation/VisionSystemSim.java | 2 +- .../java/org/photonvision/OpenCVTest.java | 4 +-- .../org/photonvision/VisionSystemSimTest.java | 34 ++++++++++--------- .../src/main/java/org/photonvision/Main.java | 2 +- .../photonvision/estimation/TargetModel.java | 4 ++- .../estimation/VisionEstimation.java | 8 ++--- 19 files changed, 98 insertions(+), 74 deletions(-) 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 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 +80,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/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/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-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( From 76e3c6d5a5514af8f58b7c798ed33a847d40f494 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 29 Oct 2023 20:03:05 -0700 Subject: [PATCH 2/6] Remove socket camera streaming (#985) Removes websocket-based camera streaming functionality. Fixes #975. This was caused by destroying the camera streams and recreating them on nickname change. Even when directly using `MJPGFrameConsumer` and the streams were exactly the same, the freeze would occur when creating a new `MjpegServer` and require a refresh. I think this is simply how cscore works? --- .../frame/consumer/MJPGFrameConsumer.java | 26 +++- .../vision/processes/VisionModule.java | 34 ++--- .../vision/videoStream/SocketVideoStream.java | 117 -------------- .../videoStream/SocketVideoStreamManager.java | 101 ------------- .../server/CameraSocketHandler.java | 143 ------------------ .../server/CameraSocketMessageType.java | 46 ------ .../java/org/photonvision/server/Server.java | 11 -- .../org/photonvision/server/UISettings.java | 26 ---- 8 files changed, 30 insertions(+), 474 deletions(-) delete mode 100644 photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java delete mode 100644 photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java delete mode 100644 photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java delete mode 100644 photon-server/src/main/java/org/photonvision/server/CameraSocketMessageType.java delete mode 100644 photon-server/src/main/java/org/photonvision/server/UISettings.java diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java index 06f9bf986b..aa6727e4e4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java @@ -28,12 +28,18 @@ import org.opencv.core.Rect; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.ColorHelper; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.opencv.CVMat; -public class MJPGFrameConsumer { - public static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3); +public class MJPGFrameConsumer implements AutoCloseable { + private static final double MAX_FRAMERATE = -1; + private static final long MAX_FRAME_PERIOD_NS = Math.round(1e9 / MAX_FRAMERATE); + private long lastFrameTimeNs; + + private static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3); private static final double EMPTY_FRAMERATE = 2; - private long lastEmptyTime; + private static final long EMPTY_FRAME_PERIOD_NS = Math.round(1e9 / EMPTY_FRAMERATE); + private long lastEmptyTimeNs; static { EMPTY_MAT.setTo(ColorHelper.colorToScalar(Color.BLACK)); @@ -168,11 +174,15 @@ public MJPGFrameConsumer(String name, int port) { public void accept(CVMat image) { if (image != null && !image.getMat().empty()) { - cvSource.putFrame(image.getMat()); + long now = MathUtils.wpiNanoTime(); + if (now - lastFrameTimeNs > 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/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/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-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; -} From 363e1d8fd47dcf0163d1bfb84a90ba6ac190e712 Mon Sep 17 00:00:00 2001 From: amquake Date: Mon, 30 Oct 2023 18:55:35 -0700 Subject: [PATCH 3/6] [photon-core] Check for multitarget params (#988) * require params in multitarget * format --- .../org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 14a6681026..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 @@ -39,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) { From 1d6abf6ba92e31834013b2ca3b925c1aa2d84108 Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 30 Oct 2023 23:01:33 -0400 Subject: [PATCH 4/6] Update README.md (#989) --- README.md | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index e513bbd4b7..968cfa6a8b 100644 --- a/README.md +++ b/README.md @@ -18,9 +18,15 @@ If you are interested in contributing code or documentation to the project, plea Note that these are case sensitive! -* `-PArchOverride=foobar`: builds for a target system other than your current architecture. Valid overrides are winx32, winx64, -macx64, macarm64, linuxx64, linuxarm64, linuxarm32, and linuxathena. -- `-PtgtIp`: deploys (builds and copies the JAR) to the coprocessor at the specified IP +* `-PArchOverride=foobar`: builds for a target system other than your current architecture. Valid overrides are: + * linuxathena + * linuxarm32 + * linuxarm64 + * arm32" + * arm64" + * x86-64" + * x86" +- `-PtgtIp`: Specifies where `./gradlew deploy` should try to copy the fat JAR to - `-Pprofile`: enables JVM profiling ## Building From fa87af9c26d87336dc8fbaf89c19332c1b613754 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com> Date: Mon, 30 Oct 2023 23:56:40 -0400 Subject: [PATCH 5/6] Update issue templates (#977) --- .github/ISSUE_TEMPLATE/bug_report.md | 31 +++++++++++++++++++++++ .github/ISSUE_TEMPLATE/feature_request.md | 20 +++++++++++++++ 2 files changed, 51 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000000..d5d59cc6f1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,31 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is and what the expected behavior should have been. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Screenshots / Videos** +If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. + +**Platform:** + - Hardware Platform (ex. Raspberry Pi 4, Windows x64): + - Network Configuration (Connection between the Radio and any devices in between, such as a Network Switch): + - PhotonVision Version: + - Browser (with Version) (Chrome, Edge, Firefox, etc.): + - Camera(s) Used: + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..11fc491ef1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. From 6666b22fc1ebc19503565f40c598960382d9da52 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 31 Oct 2023 00:10:57 -0400 Subject: [PATCH 6/6] Fix trailing quotes in readme (#990) * Fix trailing quotes in readme * Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 968cfa6a8b..556043437a 100644 --- a/README.md +++ b/README.md @@ -22,10 +22,10 @@ Note that these are case sensitive! * linuxathena * linuxarm32 * linuxarm64 - * arm32" - * arm64" - * x86-64" - * x86" + * arm32 + * arm64 + * x86-64 + * x86 - `-PtgtIp`: Specifies where `./gradlew deploy` should try to copy the fat JAR to - `-Pprofile`: enables JVM profiling