From 6d2eae7f2073fcb2f0ba615dd675da26ac5aa11d Mon Sep 17 00:00:00 2001 From: person4268 <28717044+person4268@users.noreply.github.com> Date: Sat, 3 Jun 2023 21:04:04 -0400 Subject: [PATCH] Add ignored cameras regex to command-line arguemnts (#849) This adds a regex that ignores cameras if they match it, for if you have another piece of software running that needs a second camera, or if you have a webcam in your laptop that cscore hates. --- build.gradle | 2 +- .../common/configuration/HardwareConfig.java | 8 +++++-- .../vision/pipe/impl/FindCirclesPipe.java | 1 + .../vision/pipe/impl/FindPolygonPipe.java | 1 + .../vision/processes/VisionSourceManager.java | 7 ++++++ .../processes/VisionSourceSettables.java | 1 + .../common/ShapeBenchmarkTest.java | 1 + .../vision/frame/provider/LibcameraTest.java | 1 + .../org/photonvision/PhotonPoseEstimator.java | 4 +++- .../org/photonvision/RobotPoseEstimator.java | 24 ++++++++++++++----- .../estimation/CameraTargetRelation.java | 2 ++ .../photonvision/estimation/OpenCVHelp.java | 6 +++++ .../photonvision/estimation/PNPResults.java | 2 ++ .../estimation/RotTrlTransform3d.java | 5 ++++ .../estimation/VisionEstimation.java | 1 + .../src/main/java/org/photonvision/Main.java | 10 ++++++++ .../main/java/frc/robot/AutoController.java | 4 +++- .../src/main/java/frc/robot/Drivetrain.java | 4 +++- .../frc/robot/DrivetrainPoseEstimator.java | 4 +++- .../main/java/frc/robot/DrivetrainSim.java | 4 +++- 20 files changed, 78 insertions(+), 14 deletions(-) diff --git a/build.gradle b/build.gradle index b106b96a4..0a6e4c10a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,5 +1,5 @@ plugins { - id "com.diffplug.spotless" version "6.1.2" + id "com.diffplug.spotless" version "6.19.0" id "com.github.johnrengelman.shadow" version "7.1.2" id "com.github.node-gradle.node" version "3.1.1" apply false id "edu.wpi.first.GradleJni" version "1.0.0" diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java b/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java index b990ff242..8ec2a21db 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java @@ -128,12 +128,16 @@ public class HardwareConfig { this.blacklistedResIndices = blacklistedResIndices; } - /** @return True if the FOV has been preset to a sane value, false otherwise */ + /** + * @return True if the FOV has been preset to a sane value, false otherwise + */ public final boolean hasPresetFOV() { return vendorFOV > 0; } - /** @return True if any command has been configured to a non-default empty, false otherwise */ + /** + * @return True if any command has been configured to a non-default empty, false otherwise + */ public final boolean hasCommandsConfigured() { return cpuTempCommand != "" || cpuMemoryCommand != "" diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java index ec321daa1..8113ce35d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java @@ -33,6 +33,7 @@ public class FindCirclesPipe // Output vector of found circles. Each vector is encoded as 3 or 4 element floating-point vector // (x,y,radius) or (x,y,radius,votes) . private final Mat circles = new Mat(); + /** * Runs the process for the pipe. The reason we need a separate pipe for circles is because if we * were to use the FindShapes pipe, we would have to assume that any shape more than 10-20+ sides diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindPolygonPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindPolygonPipe.java index 83d1baf59..01db9a348 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindPolygonPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindPolygonPipe.java @@ -82,6 +82,7 @@ public class FindPolygonPipe public static class FindPolygonPipeParams { private final double accuracyPercentage; + // Should be a value between 0-100 public FindPolygonPipeParams(double accuracyPercentage) { this.accuracyPercentage = accuracyPercentage; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 81f216922..3d9c27046 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -44,6 +44,7 @@ public class VisionSourceManager { final List knownUsbCameras = new CopyOnWriteArrayList<>(); final List unmatchedLoadedConfigs = new CopyOnWriteArrayList<>(); private boolean hasWarned; + private String ignoredCamerasRegex = ""; private static class SingletonHolder { private static final VisionSourceManager INSTANCE = new VisionSourceManager(); @@ -282,12 +283,18 @@ public class VisionSourceManager { return cfg; } + public void setIgnoredCamerasRegex(String ignoredCamerasRegex) { + this.ignoredCamerasRegex = ignoredCamerasRegex; + } + private List filterAllowedDevices(List allDevices) { List filteredDevices = new ArrayList<>(); for (var device : allDevices) { if (deviceBlacklist.contains(device.name)) { logger.trace( "Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\""); + } else if (device.name.matches(ignoredCamerasRegex)) { + logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path); } else { filteredDevices.add(device); logger.trace( diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index bbabf6ba9..63e643c8b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -49,6 +49,7 @@ public abstract class VisionSourceSettables { public abstract void setBrightness(int brightness); public abstract void setGain(int gain); + // Pretty uncommon so instead of abstract this is just a no-op by default // Overriden by cameras with AWB gain support public void setRedGain(int red) {} diff --git a/photon-core/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java b/photon-core/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java index 8b83dc776..6ce478111 100644 --- a/photon-core/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java +++ b/photon-core/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java @@ -16,6 +16,7 @@ */ package org.photonvision.common; + /* * Copyright (C) 2020 Photon Vision. * diff --git a/photon-core/src/test/java/org/photonvision/vision/frame/provider/LibcameraTest.java b/photon-core/src/test/java/org/photonvision/vision/frame/provider/LibcameraTest.java index e72d84b03..b23a1cd9b 100644 --- a/photon-core/src/test/java/org/photonvision/vision/frame/provider/LibcameraTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/frame/provider/LibcameraTest.java @@ -18,6 +18,7 @@ package org.photonvision.vision.frame.provider; import org.junit.jupiter.api.Test; + // import org.photonvision.raspi.LibCameraJNI; public class LibcameraTest { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index ca780cf2d..119aa3155 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -233,7 +233,9 @@ public class PhotonPoseEstimator { setLastPose(new Pose3d(lastPose)); } - /** @return The current transform from the center of the robot to the camera mount position */ + /** + * @return The current transform from the center of the robot to the camera mount position + */ public Transform3d getRobotToCameraTransform() { return robotToCamera; } diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java index f784acdcf..4dd6dcdaa 100644 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java @@ -40,7 +40,9 @@ import java.util.Set; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -/** @deprecated Use {@link PhotonPoseEstimator} */ +/** + * @deprecated Use {@link PhotonPoseEstimator} + */ @Deprecated public class RobotPoseEstimator { /** @@ -345,27 +347,37 @@ public class RobotPoseEstimator { return x.getTranslation().getDistance(y.getTranslation()); } - /** @param aprilTags the aprilTags to set */ + /** + * @param aprilTags the aprilTags to set + */ public void setAprilTags(AprilTagFieldLayout aprilTags) { this.aprilTags = aprilTags; } - /** @return the aprilTags */ + /** + * @return the aprilTags + */ public AprilTagFieldLayout getAprilTags() { return aprilTags; } - /** @return the strategy */ + /** + * @return the strategy + */ public PoseStrategy getStrategy() { return strategy; } - /** @param strategy the strategy to set */ + /** + * @param strategy the strategy to set + */ public void setStrategy(PoseStrategy strategy) { this.strategy = strategy; } - /** @return the referencePose */ + /** + * @return the referencePose + */ public Pose3d getReferencePose() { return referencePose; } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java index 417bffa04..40232aa8b 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java @@ -36,12 +36,14 @@ public class CameraTargetRelation { public final double camToTargDistXY; public final Rotation2d camToTargYaw; public final Rotation2d camToTargPitch; + /** Angle from the camera's relative x-axis */ public final Rotation2d camToTargAngle; public final Transform3d targToCam; public final Rotation2d targToCamYaw; public final Rotation2d targToCamPitch; + /** Angle from the target's relative x-axis */ public final Rotation2d targToCamAngle; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 05e13c9e3..acd9a71c2 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -99,6 +99,7 @@ public final class OpenCVHelp { } return new MatOfPoint3f(points); } + /** * Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three * elements representing {x, y, z} in the EDN coordinate system. @@ -128,6 +129,7 @@ public final class OpenCVHelp { rotation = rotationNWUtoEDN(rotation); return new MatOfPoint3f(new Point3(rotation.getQuaternion().toRotationVector().getData())); } + /** * Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three * elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm, @@ -228,6 +230,7 @@ public final class OpenCVHelp { new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN()) .plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU())); } + /** * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN, * this would be XYZ {0, -1, 0} in NWU. @@ -324,6 +327,7 @@ public final class OpenCVHelp { corn.release(); return rect; } + /** * Gets the rotated rectangle with minimum area which bounds this contour. * @@ -338,6 +342,7 @@ public final class OpenCVHelp { corn.release(); return rect; } + /** * Get the area in pixels of this target's contour. It's important to note that this may be * different from the area of the bounding rectangle around the contour. @@ -451,6 +456,7 @@ public final class OpenCVHelp { if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); else return new PNPResults(best, errors[0]); } + /** * Finds the transformation that maps the camera's pose to the target pose. The camera's pose * relative to the target is determined by the supplied 3d points of the target's model and their diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java index 2471d5953..6a88a4d5f 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java @@ -44,8 +44,10 @@ public class PNPResults { * to the best solution. */ public final Transform3d alt; + /** If no alternate solution is found, this is bestReprojErr */ public final double altReprojErr; + /** If no alternate solution is found, this is 0 */ public final double ambiguity; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index d6d5e2e12..549614de8 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -41,6 +41,7 @@ public class RotTrlTransform3d { public RotTrlTransform3d() { this(new Rotation3d(), new Translation3d()); } + /** * Creates a rotation-translation transformation from a Transform3d. * @@ -52,6 +53,7 @@ public class RotTrlTransform3d { public RotTrlTransform3d(Transform3d trf) { this(trf.getRotation(), trf.getTranslation()); } + /** * A rotation-translation transformation. * @@ -65,6 +67,7 @@ public class RotTrlTransform3d { this.rot = rot; this.trl = trl; } + /** * The rotation-translation transformation that makes poses in the world consider this pose as the * new origin, or change the basis to this pose. @@ -86,10 +89,12 @@ public class RotTrlTransform3d { public Transform3d getTransform() { return new Transform3d(trl, rot); } + /** The translation component of this transformation */ public Translation3d getTranslation() { return trl; } + /** The rotation component of this transformation */ public Rotation3d getRotation() { return rot; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 33db1a6c2..82051d8bc 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -153,6 +153,7 @@ public class VisionEstimation { */ public static class SVDResults { public final RotTrlTransform3d trf; + /** If the result is invalid, this value is -1 */ public final double rmse; diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 2f5d8f3d0..175ee2703 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -75,6 +75,11 @@ public class Main { "Run in test mode with 2019 and 2020 WPI field images in place of cameras"); options.addOption("p", "path", true, "Point test mode to a specific folder"); + options.addOption( + "i", + "ignore-cameras", + true, + "Ignore cameras that match a regex. Uses camera name as provided by cscore."); CommandLineParser parser = new DefaultParser(); CommandLine cmd = parser.parse(options, args); @@ -99,6 +104,11 @@ public class Main { testModeFolder = p; } } + + if (cmd.hasOption("ignore-cameras")) { + VisionSourceManager.getInstance() + .setIgnoredCamerasRegex(cmd.getOptionValue("ignore-cameras")); + } } return true; } diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java index 113756561..bb0ee5a9b 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java @@ -61,7 +61,9 @@ public class AutoController { new TrajectoryConfig(2, 2)); } - /** @return The starting (initial) pose of the currently-active trajectory */ + /** + * @return The starting (initial) pose of the currently-active trajectory + */ public Pose2d getInitialPose() { return trajectory.getInitialPose(); } diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java index dc8d5e0e3..996739c95 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java @@ -130,7 +130,9 @@ public class Drivetrain { poseEst.resetToPose(pose, leftEncoder.getDistance(), rightEncoder.getDistance()); } - /** @return The current best-guess at drivetrain Pose on the field. */ + /** + * @return The current best-guess at drivetrain Pose on the field. + */ public Pose2d getCtrlsPoseEstimate() { return poseEst.getPoseEst(); } diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java index 0a4de26e9..1b28def5f 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -100,7 +100,9 @@ public class DrivetrainPoseEstimator { m_poseEstimator.resetPosition(gyro.getRotation2d(), leftDist, rightDist, pose); } - /** @return The current best-guess at drivetrain position on the field. */ + /** + * @return The current best-guess at drivetrain position on the field. + */ public Pose2d getPoseEst() { return m_poseEstimator.getEstimatedPosition(); } diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java index 2e18ee030..2eeaaa106 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java @@ -149,7 +149,9 @@ public class DrivetrainSim { drivetrainSimulator.setPose(pose); } - /** @return The simulated robot's position, in meters. */ + /** + * @return The simulated robot's position, in meters. + */ public Pose2d getCurPose() { return drivetrainSimulator.getPose(); }