diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index e2c3d773cc..94a5e5f075 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -106,14 +106,14 @@ jobs: run: | chmod +x gradlew ./gradlew copyPhotonlib -x check - ./gradlew buildAllExamples -x check --max-workers 2 + ./gradlew build -x check --max-workers 2 - name: Build C++ examples working-directory: photonlib-cpp-examples run: | chmod +x gradlew ./gradlew copyPhotonlib -x check - ./gradlew buildAllExamples -x check --max-workers 2 + ./gradlew build -x check --max-workers 2 photon-build-all: # The type of runner that the job will run on. runs-on: ubuntu-22.04 @@ -225,6 +225,7 @@ jobs: include: - os: windows-2022 artifact-name: Win64 + architecture: x64 # Mac builds are broken due to opencv class loading issues -- disable for now # - os: macos-11 # artifact-name: macOS @@ -255,7 +256,7 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2023-22.04 + - container: wpilib/roborio-cross-ubuntu:2024-22.04 artifact-name: Athena - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 artifact-name: Raspbian diff --git a/LICENSE_MathUtils_orthogonalizeRotationMatrix.txt b/LICENSE_MathUtils_orthogonalizeRotationMatrix.txt deleted file mode 100644 index 73f837d96b..0000000000 --- a/LICENSE_MathUtils_orthogonalizeRotationMatrix.txt +++ /dev/null @@ -1,23 +0,0 @@ -Copyright (c) Photon Vision. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle index 0225b7003e..199729e75a 100644 --- a/build.gradle +++ b/build.gradle @@ -26,9 +26,12 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2023.4.2" - opencvVersion = "4.8.0-1" + wpilibVersion = "2024.1.1-beta-1" + openCVversion = "4.8.0-1" joglVersion = "2.4.0-rc-20200307" + javalinVersion = "5.6.2" + frcYear = "2024" + pubVersion = versionString isDev = pubVersion.startsWith("dev") @@ -60,3 +63,7 @@ spotless { targetExclude("photon-lib/src/main/java/org/photonvision/PhotonVersion.java") } } + +wrapper { + gradleVersion '8.3' +} diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index ae04661ee7..7f959b087a 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew index 8bedea1ac8..89a57c4dfe 100755 --- a/gradlew +++ b/gradlew @@ -36,7 +36,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photon-client/src/assets/images/loading.svg b/photon-client/src/assets/images/loading.svg index 7edbc87ee4..a471c1d0f7 100644 --- a/photon-client/src/assets/images/loading.svg +++ b/photon-client/src/assets/images/loading.svg @@ -1,4 +1,5 @@ - + diff --git a/photon-client/src/assets/images/logoLarge.svg b/photon-client/src/assets/images/logoLarge.svg index b682db7603..d6c2247bf0 100644 --- a/photon-client/src/assets/images/logoLarge.svg +++ b/photon-client/src/assets/images/logoLarge.svg @@ -1,6 +1,8 @@ - + diff --git a/photon-client/src/assets/images/logoSmall.svg b/photon-client/src/assets/images/logoSmall.svg index 88643de263..f528a70023 100644 --- a/photon-client/src/assets/images/logoSmall.svg +++ b/photon-client/src/assets/images/logoSmall.svg @@ -1,6 +1,8 @@ - + diff --git a/photon-client/src/components/app/photon-3d-visualizer.vue b/photon-client/src/components/app/photon-3d-visualizer.vue index e723eecd2c..caa3070ccb 100644 --- a/photon-client/src/components/app/photon-3d-visualizer.vue +++ b/photon-client/src/components/app/photon-3d-visualizer.vue @@ -4,18 +4,18 @@ import { onBeforeUnmount, onMounted, watchEffect } from "vue"; import { ArrowHelper, BoxGeometry, + Color, ConeGeometry, Mesh, MeshNormalMaterial, + type Object3D, PerspectiveCamera, Quaternion, Scene, Vector3, - Color, WebGLRenderer } from "three"; import { TrackballControls } from "three/examples/jsm/controls/TrackballControls"; -import { type Object3D } from "three"; const props = defineProps<{ targets: PhotonTarget[]; diff --git a/photon-client/src/components/app/photon-log-view.vue b/photon-client/src/components/app/photon-log-view.vue index 88ec22e1ae..e5442bc28c 100644 --- a/photon-client/src/components/app/photon-log-view.vue +++ b/photon-client/src/components/app/photon-log-view.vue @@ -1,5 +1,5 @@

The following function is released under the BSD license avaliable in - * LICENSE_MathUtils_orthogonalizeRotationMatrix.txt. - */ - public static Matrix orthogonalizeRotationMatrix(Matrix input) { - var a = DecompositionFactory_DDRM.qr(3, 3); - if (!a.decompose(input.getStorage().getDDRM())) { - // best we can do is return the input - return input; - } - - // Grab results (thanks for this _great_ api, EJML) - var Q = new DMatrixRMaj(3, 3); - var R = new DMatrixRMaj(3, 3); - a.getQ(Q, false); - a.getR(R, false); - - // Fix signs in R if they're < 0 so it's close to an identity matrix - // (our QR decomposition implementation sometimes flips the signs of columns) - for (int colR = 0; colR < 3; ++colR) { - if (R.get(colR, colR) < 0) { - for (int rowQ = 0; rowQ < 3; ++rowQ) { - Q.set(rowQ, colR, -Q.get(rowQ, colR)); - } - } - } - - return new Matrix<>(new SimpleMatrix(Q)); - } } diff --git a/photon-core/src/main/java/org/photonvision/common/util/numbers/NumberCouple.java b/photon-core/src/main/java/org/photonvision/common/util/numbers/NumberCouple.java index e555e361f9..9a6047c797 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/numbers/NumberCouple.java +++ b/photon-core/src/main/java/org/photonvision/common/util/numbers/NumberCouple.java @@ -60,11 +60,7 @@ public boolean equals(Object obj) { return false; } - if (!couple.second.equals(second)) { - return false; - } - - return true; + return couple.second.equals(second); } @JsonIgnore diff --git a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java index 4ebc2c8735..a75ae2e579 100644 --- a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java +++ b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java @@ -28,7 +28,7 @@ public class LibCameraJNI { private static boolean libraryLoaded = false; - private static Logger logger = new Logger(LibCameraJNI.class, LogGroup.Camera); + private static final Logger logger = new Logger(LibCameraJNI.class, LogGroup.Camera); public static final Object CAMERA_LOCK = new Object(); @@ -120,7 +120,7 @@ public static boolean isSupported() { /** * Starts the camera thresholder and display threads running. Make sure that this function is - * called syncronously with stopCamera and returnFrame! + * called synchronously with stopCamera and returnFrame! */ public static native boolean startCamera(); @@ -141,7 +141,7 @@ public static native boolean setThresholds( // Exposure time, in microseconds public static native boolean setExposure(int exposureUs); - // Set brighness on [-1, 1] + // Set brightness on [-1, 1] public static native boolean setBrightness(double brightness); // Unknown ranges for red and blue AWB gain @@ -164,18 +164,18 @@ public static native boolean setThresholds( // Analog gain multiplier to apply to all color channels, on [1, Big Number] public static native boolean setAnalogGain(double analog); - /** Block until a new frame is avaliable from native code. */ + /** Block until a new frame is available from native code. */ public static native boolean awaitNewFrame(); /** - * Get a pointer to the most recent color mat generated. Call this immediatly after awaitNewFrame, - * and call onlly once per new frame! + * Get a pointer to the most recent color mat generated. Call this immediately after + * awaitNewFrame, and call only once per new frame! */ public static native long takeColorFrame(); /** - * Get a pointer to the most recent processed mat generated. Call this immediatly after - * awaitNewFrame, and call onlly once per new frame! + * Get a pointer to the most recent processed mat generated. Call this immediately after + * awaitNewFrame, and call only once per new frame! */ public static native long takeProcessedFrame(); @@ -186,6 +186,6 @@ public static native boolean setThresholds( public static native int getGpuProcessType(); - // /** Release a frame pointer back to the libcamera driver code to be filled again */ + // Release a frame pointer back to the libcamera driver code to be filled again */ // public static native long returnFrame(long frame); } diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java index afbfc463a2..8fbda02683 100644 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java +++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java @@ -28,7 +28,7 @@ public enum AprilTagFamily { kTagCustom48h11; public String getNativeName() { - // We wanna strip the leading kT and replace with "t" + // We want to strip the leading kT and replace with "t" return this.name().replaceFirst("kT", "t"); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java b/photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java index 810afe0536..24ea8dafab 100644 --- a/photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java +++ b/photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java @@ -19,6 +19,7 @@ import org.opencv.objdetect.ArucoDetector; import org.opencv.objdetect.DetectorParameters; +import org.opencv.objdetect.Objdetect; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; @@ -37,8 +38,9 @@ public ArucoDetectorParams() { setCornerAccuracy(25); setCornerRefinementMaxIterations(100); - // TODO - // detector = new ArucoDetector(Dictionary.get(Aruco.DICT_APRILTAG_16h5), parameters); + detector = + new ArucoDetector( + Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5), parameters); } public void setDecimation(float decimate) { @@ -46,17 +48,16 @@ public void setDecimation(float decimate) { logger.info("Setting decimation from " + m_decimate + " to " + decimate); - // We only need to mutate the parameters -- the detector keeps a poitner to the parameters + // We only need to mutate the parameters -- the detector keeps a pointer to the parameters // object internally, so it should automatically update - parameters.set_aprilTagQuadDecimate((float) decimate); + parameters.set_aprilTagQuadDecimate(decimate); m_decimate = decimate; } public void setCornerRefinementMaxIterations(int iters) { if (iters == m_iterations || iters <= 0) return; - // TODO - // parameters.set_cornerRefinementMethod(Aruco.CORNER_REFINE_SUBPIX); + parameters.set_cornerRefinementMethod(Objdetect.CORNER_REFINE_SUBPIX); parameters.set_cornerRefinementMaxIterations(iters); // 200 m_iterations = iters; diff --git a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java index 0963a2812a..6bb10f7baa 100644 --- a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java +++ b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java @@ -58,7 +58,7 @@ public PhotonArucoDetector() { ids = new Mat(); tvecs = new Mat(); rvecs = new Mat(); - corners = new ArrayList(); + corners = new ArrayList<>(); tagPose = new Pose3d(); translation = new Translation3d(); rotation = new Rotation3d(); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index e028dfe369..951135fcf4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -35,7 +35,7 @@ public class FileVisionSource extends VisionSource { public FileVisionSource(CameraConfiguration cameraConfiguration) { super(cameraConfiguration); var calibration = - cameraConfiguration.calibrations.size() > 0 + !cameraConfiguration.calibrations.isEmpty() ? cameraConfiguration.calibrations.get(0) : null; frameProvider = diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 22c76131aa..7e0bd5c5c7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -132,7 +132,7 @@ public void setExposure(double exposure) { // If we set exposure too low, libcamera crashes or slows down // Very weird and smelly // For now, band-aid this by just not setting it lower than the "it breaks" limit - // Limit is different depending on camera. + // is different depending on camera. if (sensorModel == LibCameraJNI.SensorModel.OV9281) { if (exposure < 6.0) { exposure = 6.0; diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 780a12786e..da2dc835cb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -105,7 +105,7 @@ public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid) { public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseName) { for (var qc : quirkyCameras) { - boolean hasBaseName = !qc.baseName.equals(""); + boolean hasBaseName = !qc.baseName.isEmpty(); boolean matchesBaseName = qc.baseName.equals(baseName) || !hasBaseName; if (qc.usbVid == usbVid && qc.usbPid == usbPid && matchesBaseName) { return qc; diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index cdad1ca24e..ad235da26a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -18,7 +18,10 @@ package org.photonvision.vision.camera; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.*; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoException; +import edu.wpi.first.cscore.VideoMode; import java.util.*; import java.util.stream.Collectors; import org.photonvision.common.configuration.CameraConfiguration; @@ -120,8 +123,8 @@ public void setAutoExposure(boolean cameraAutoExposure) { if (!cameraAutoExposure) { // Pick a bunch of reasonable setting defaults for vision processing retroreflective camera.getProperty("auto_exposure_bias").set(0); - camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement - camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement + camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustment + camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustment camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled camera.getProperty("auto_exposure").set(1); // auto exposure disabled } else { @@ -129,7 +132,7 @@ public void setAutoExposure(boolean cameraAutoExposure) { // nice-for-humans camera.getProperty("auto_exposure_bias").set(12); camera.getProperty("iso_sensitivity_auto").set(1); - camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default + camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustment by default camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled camera.getProperty("auto_exposure").set(0); // auto exposure enabled } @@ -179,15 +182,14 @@ public void setExposure(double exposure) { try { int scaledExposure = 1; if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { - scaledExposure = - (int) Math.round(timeToPiCamRawExposure(pctToExposureTimeUs(exposure))); - logger.debug("Setting camera raw exposure to " + Integer.toString(scaledExposure)); + scaledExposure = Math.round(timeToPiCamRawExposure(pctToExposureTimeUs(exposure))); + logger.debug("Setting camera raw exposure to " + scaledExposure); camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); } else { scaledExposure = (int) Math.round(exposure); - logger.debug("Setting camera exposure to " + Integer.toString(scaledExposure)); + logger.debug("Setting camera exposure to " + scaledExposure); camera.setExposureManual(scaledExposure); camera.setExposureManual(scaledExposure); } @@ -264,9 +266,7 @@ public HashMap getAllVideoModes() { } else { modes = camera.enumerateVideoModes(); } - for (int i = 0; i < modes.length; i++) { - var videoMode = modes[i]; - + for (VideoMode videoMode : modes) { // Filter grey modes if (videoMode.pixelFormat == VideoMode.PixelFormat.kGray || videoMode.pixelFormat == VideoMode.PixelFormat.kUnknown) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java index 3c405632de..70de9c5115 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java @@ -24,15 +24,15 @@ public interface FrameProvider extends Supplier { String getName(); - /** Ask the camera to produce a certain kind of processed image (eg HSV or greyscale) */ - public void requestFrameThresholdType(FrameThresholdType type); + /** Ask the camera to produce a certain kind of processed image (e.g. HSV or greyscale) */ + void requestFrameThresholdType(FrameThresholdType type); /** Ask the camera to rotate frames it outputs */ - public void requestFrameRotation(ImageRotationMode rotationMode); + void requestFrameRotation(ImageRotationMode rotationMode); /** Ask the camera to provide either the input, output, or both frames. */ - public void requestFrameCopies(boolean copyInput, boolean copyOutput); + void requestFrameCopies(boolean copyInput, boolean copyOutput); /** Ask the camera to rotate frames it outputs */ - public void requestHsvSettings(HSVPipe.HSVParams params); + void requestHsvSettings(HSVPipe.HSVParams params); } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java index e9773122a0..2552b5fbdb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java @@ -33,8 +33,8 @@ public class FileSaveFrameConsumer implements Consumer { // Formatters to generate unique, timestamped file names - private static String FILE_PATH = ConfigManager.getInstance().getImageSavePath().toString(); - private static String FILE_EXTENSION = ".jpg"; + private static final String FILE_PATH = ConfigManager.getInstance().getImageSavePath().toString(); + private static final String FILE_EXTENSION = ".jpg"; DateFormat df = new SimpleDateFormat("yyyy-MM-dd"); DateFormat tf = new SimpleDateFormat("hhmmssSS"); private final String NT_SUFFIX = "SaveImgCmd"; @@ -44,7 +44,7 @@ public class FileSaveFrameConsumer implements Consumer { private final Logger logger; private long imgSaveCountInternal = 0; private String camNickname; - private String fnamePrefix; + private final String fnamePrefix; private IntegerEntry entry; public FileSaveFrameConsumer(String camNickname, String streamPrefix) { 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 b070e372d5..06f9bf986b 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 @@ -106,7 +106,6 @@ public class MJPGFrameConsumer { private CvSource cvSource; private MjpegServer mjpegServer; - @SuppressWarnings("FieldCanBeLocal") private VideoListener listener; private final NetworkTable table; diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index f9f28c0a51..034ae6615e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -30,7 +30,7 @@ import org.photonvision.vision.pipe.impl.RotateImagePipe; public abstract class CpuImageProcessor implements FrameProvider { - protected class CapturedFrame { + protected static class CapturedFrame { CVMat colorImage; FrameStaticProperties staticProps; long captureTimestamp; @@ -119,6 +119,5 @@ public void requestHsvSettings(HSVPipe.HSVParams params) { @Override public void requestFrameCopies(boolean copyInput, boolean copyOutput) { // We don't actually do zero-copy, so this method is a no-op - return; } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index 8c7272ba21..315e4e3de3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -63,7 +63,7 @@ public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients ca public FileFrameProvider( Path path, double fov, int maxFPS, CameraCalibrationCoefficients calibration) { if (!Files.exists(path)) - throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString()); + throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath()); this.path = path; this.millisDelay = 1000 / maxFPS; diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/CVShape.java b/photon-core/src/main/java/org/photonvision/vision/opencv/CVShape.java index 71f21cc1d4..99b8c2f107 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/CVShape.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/CVShape.java @@ -32,7 +32,7 @@ public class CVShape implements Releasable { private MatOfPoint3f customTarget = null; - private MatOfPoint2f approxCurve = new MatOfPoint2f(); + private final MatOfPoint2f approxCurve = new MatOfPoint2f(); public CVShape(Contour contour, ContourShape shape) { this.contour = contour; diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/Contour.java b/photon-core/src/main/java/org/photonvision/vision/opencv/Contour.java index ea89c8b50a..181de0b3b3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/Contour.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/Contour.java @@ -21,13 +21,7 @@ import java.util.Collection; import java.util.Comparator; import org.jetbrains.annotations.Nullable; -import org.opencv.core.CvType; -import org.opencv.core.MatOfInt; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.core.RotatedRect; +import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.opencv.imgproc.Moments; import org.photonvision.common.util.math.MathUtils; diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/ContourSortMode.java b/photon-core/src/main/java/org/photonvision/vision/opencv/ContourSortMode.java index d4cf359e49..2ceff908ec 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/ContourSortMode.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/ContourSortMode.java @@ -35,7 +35,7 @@ public enum ContourSortMode { (Math.pow(rect.getMinAreaRect().center.y, 2) + Math.pow(rect.getMinAreaRect().center.x, 2)))); - private Comparator m_comparator; + private final Comparator m_comparator; ContourSortMode(Comparator comparator) { m_comparator = comparator; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java index f7c8ab2b20..ab52d7bcfb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java @@ -46,8 +46,7 @@ public boolean equals(Object obj) { AprilTagDetectionPipeParams other = (AprilTagDetectionPipeParams) obj; if (family != other.family) return false; if (detectorParams == null) { - if (other.detectorParams != null) return false; - } else if (!detectorParams.equals(other.detectorParams)) return false; - return true; + return other.detectorParams == null; + } else return detectorParams.equals(other.detectorParams); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 1dc118a3eb..9738202e3e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -46,7 +46,7 @@ public AprilTagPoseEstimatorPipe() { @Override protected AprilTagPoseEstimate process(AprilTagDetection in) { // Save the corner points of our detection to an array - Point corners[] = new Point[4]; + Point[] corners = new Point[4]; for (int i = 0; i < 4; i++) { corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); } @@ -128,8 +128,7 @@ public boolean equals(Object obj) { if (config == null) { if (other.config != null) return false; } else if (!config.equals(other.config)) return false; - if (nIters != other.nIters) return false; - return true; + return nIters == other.nIters; } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java index 7789c70b60..113da4d357 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java @@ -23,7 +23,7 @@ public class CalculateFPSPipe extends CVPipe { - private LinearFilter fpsFilter = LinearFilter.movingAverage(20); + private final LinearFilter fpsFilter = LinearFilter.movingAverage(20); StopWatch clock = new StopWatch(); @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java index a454400bb9..bfca09d6fd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java @@ -24,7 +24,9 @@ import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Triple; import org.opencv.calib3d.Calib3d; -import org.opencv.core.*; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.Size; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; @@ -38,24 +40,24 @@ public class Calibrate3dPipe Calibrate3dPipe.CalibratePipeParams> { // Camera matrix stores the center of the image and focal length across the x and y-axis in a 3x3 // matrix - private Mat cameraMatrix = new Mat(); + private final Mat cameraMatrix = new Mat(); // Stores the radical and tangential distortion in a 5x1 matrix - private MatOfDouble distortionCoefficients = new MatOfDouble(); + private final MatOfDouble distortionCoefficients = new MatOfDouble(); - // For loggging + // For logging private static final Logger logger = new Logger(Calibrate3dPipe.class, LogGroup.General); // Translational and rotational matrices - private List rvecs = new ArrayList<>(); - private List tvecs = new ArrayList<>(); + private final List rvecs = new ArrayList<>(); + private final List tvecs = new ArrayList<>(); // The Standard deviation of the estimated parameters - private Mat stdDeviationsIntrinsics = new Mat(); - private Mat stdDeviationsExtrinsics = new Mat(); + private final Mat stdDeviationsIntrinsics = new Mat(); + private final Mat stdDeviationsExtrinsics = new Mat(); // Contains the re projection error of each snapshot by re projecting the corners we found and - // finding the euclidean distance between the actual corners. - private Mat perViewErrors = new Mat(); + // finding the Euclidean distance between the actual corners. + private final Mat perViewErrors = new Mat(); // RMS of the calibration private double calibrationAccuracy; @@ -135,7 +137,7 @@ protected CameraCalibrationCoefficients process(List> in) } // Calculate standard deviation of the RMS error of the snapshots - private static double calculateSD(double numArray[]) { + private static double calculateSD(double[] numArray) { double sum = 0.0, standardDeviation = 0.0; int length = numArray.length; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java index ed3ae46e54..ff7f6fd3ea 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java @@ -18,10 +18,7 @@ package org.photonvision.vision.pipe.impl; import edu.wpi.first.math.geometry.Translation2d; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; -import java.util.List; +import java.util.*; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; @@ -42,18 +39,10 @@ protected List process(List targetList) { for (var target : targetList) { // detect corners. Might implement more algorithms later but // APPROX_POLY_DP_AND_EXTREME_CORNERS should be year agnostic - switch (params.cornerDetectionStrategy) { - case APPROX_POLY_DP_AND_EXTREME_CORNERS: - { - var targetCorners = - detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls); - target.setTargetCorners(targetCorners); - break; - } - default: - { - break; - } + if (Objects.requireNonNull(params.cornerDetectionStrategy) + == DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS) { + var targetCorners = detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls); + target.setTargetCorners(targetCorners); } } return targetList; @@ -133,7 +122,7 @@ private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo we want a number between 0 and 0.16 out of a percentage from 0 to 100 so take accuracy and divide by 600 - Furthermore, we know that the contour is open if we haven't done convex hulls + Furthermore, we know that the contour is open if we haven't done convex hulls, and it has subcontours. */ var isOpen = !convexHull && target.hasSubContours(); @@ -158,7 +147,7 @@ private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo var distanceToTrComparator = Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3))); - // top left and top right are the poly corners closest to the bouding box tl and tr + // top left and top right are the poly corners closest to the bounding box tl and tr pointList.sort(distanceToTlComparator); var tl = pointList.get(0); pointList.remove(tl); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java index 913ad5dd22..53f5852e72 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -17,7 +17,7 @@ package org.photonvision.vision.pipe.impl; -import java.awt.Color; +import java.awt.*; import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Mat; @@ -62,7 +62,7 @@ protected Void process(Pair> in) { } break; case Dual: - if (in.getRight().size() >= 1) { + if (!in.getRight().isEmpty()) { var target = in.getRight().get(0); if (target != null) { var area = target.getArea(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index a3668f99de..b88bd0ba17 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -22,12 +22,8 @@ import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.opencv.calib3d.Calib3d; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.MatOfPoint3f; +import org.opencv.core.*; import org.opencv.core.Point; -import org.opencv.core.Point3; import org.opencv.imgproc.Imgproc; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; @@ -95,7 +91,7 @@ protected Void process(Pair> in) { jac); if (params.redistortPoints) { - // Distort the points so they match the image they're being overlaid on + // Distort the points, so they match the image they're being overlaid on distortPoints(tempMat, tempMat); } @@ -111,7 +107,7 @@ protected Void process(Pair> in) { jac); if (params.redistortPoints) { - // Distort the points so they match the image they're being overlaid on + // Distort the points, so they match the image they're being overlaid on distortPoints(tempMat, tempMat); } var topPoints = tempMat.toList(); @@ -119,7 +115,7 @@ protected Void process(Pair> in) { dividePointList(bottomPoints); dividePointList(topPoints); - // floor, then pillers, then top + // floor, then pillars, then top for (int i = 0; i < bottomPoints.size(); i++) { Imgproc.line( in.getLeft(), @@ -241,11 +237,11 @@ private void distortPoints(MatOfPoint2f src, MatOfPoint2f dst) { double r2 = x * x + y * y; // square of the radius from center - // Radial distorsion + // Radial distortion double xDistort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); double yDistort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); - // Tangential distorsion + // Tangential distortion xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x)); yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java index 9a5e38fac7..678566c707 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java @@ -39,8 +39,8 @@ public class FindBoardCornersPipe Size imageSize; Size patternSize; - // Configure the optimizations used while using openCV's find corners algorithm - // Since we return results in real-time, we want ensure it goes as fast as possible + // Configure the optimizations used while using OpenCV's find corners algorithm + // Since we return results in real-time, we want to ensure it goes as fast as possible // and fails as fast as possible. final int findChessboardFlags = Calib3d.CALIB_CB_NORMALIZE_IMAGE @@ -48,9 +48,9 @@ public class FindBoardCornersPipe | Calib3d.CALIB_CB_FILTER_QUADS | Calib3d.CALIB_CB_FAST_CHECK; - private MatOfPoint2f boardCorners = new MatOfPoint2f(); + private final MatOfPoint2f boardCorners = new MatOfPoint2f(); - // Intermedeate result mat's + // Intermediate result mat's Mat smallerInFrame = new Mat(); MatOfPoint2f smallerBoardCorners = new MatOfPoint2f(); @@ -213,7 +213,7 @@ private Size getWindowSize(MatOfPoint2f inPoints) { } /** - * Find chessboard corners given a input mat and output mat to draw on + * Find chessboard corners given an input mat and output mat to draw on * * @return Frame resolution, object points, board corners */ @@ -223,7 +223,7 @@ private Triple findBoardCorners(Pair in) { var inFrame = in.getLeft(); var outFrame = in.getRight(); - // Convert the inFrame to grayscale to increase contrast + // Convert the inFrame too grayscale to increase contrast Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_BGR2GRAY); boolean boardFound = false; @@ -328,8 +328,7 @@ public boolean equals(Object obj) { if (type != other.type) return false; if (Double.doubleToLongBits(gridSize) != Double.doubleToLongBits(other.gridSize)) return false; - if (divisor != other.divisor) return false; - return true; + return divisor == other.divisor; } } } 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 8113ce35dd..07ec9ecd82 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 @@ -70,7 +70,7 @@ protected List process(Pair> in) { Math.max(1.0, params.accuracy), minRadius, maxRadius); - // Great, we now found the center point of the circle and it's radius, but we have no idea what + // Great, we now found the center point of the circle, and it's radius, but we have no idea what // contour it corresponds to // Each contour can only match to one circle, so we keep a list of unmatched contours around and // only match against them @@ -121,7 +121,7 @@ public static class FindCirclePipeParams { * If the parameter is too small, multiple neighbor circles may be falsely detected in addition to a true one. If it is too large, some circles may be missed. * * @param maxCannyThresh -First method-specific parameter. In case of #HOUGH_GRADIENT and #HOUGH_GRADIENT_ALT, it is the higher threshold of the two passed to the Canny edge detector (the lower one is twice smaller). - * Note that #HOUGH_GRADIENT_ALT uses #Scharr algorithm to compute image derivatives, so the threshold value shough normally be higher, such as 300 or normally exposed and contrasty images. + * Note that #HOUGH_GRADIENT_ALT uses #Scharr algorithm to compute image derivatives, so the threshold value should normally be higher, such as 300 or normally exposed and contrasty images. * * * @param allowableThreshold - When finding the corresponding contour, this is used to see how close a center should be to a contour for it to be considered THAT contour. 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 01db9a3480..dfadfda53f 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 @@ -19,7 +19,6 @@ import java.util.ArrayList; import java.util.List; -import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.photonvision.vision.opencv.CVShape; import org.photonvision.vision.opencv.Contour; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java index 42e8a8567c..5b49faf641 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java @@ -76,7 +76,7 @@ public class GPUAcceleratedHSVPipe extends CVPipe { "", "void main() {", " vec2 uv = gl_FragCoord.xy/resolution;", - // Important! We do this .bgr swizzle because the image comes in as BGR but we pretend + // Important! We do this .bgr swizzle because the image comes in as BGR, but we pretend // it's RGB for convenience+speed " vec3 col = texture2D(texture0, uv).bgr;", // Only the first value in the vec4 gets used for GL_RED, and only the last value gets @@ -230,7 +230,7 @@ public GPUAcceleratedHSVPipe(PBOMode pboMode) { + "', version '" + gl.glGetString(GL.GL_VERSION) + "', and profile '" - + profile.toString() + + profile + "'"); var fmt = GLBuffers.newDirectIntBuffer(1); @@ -242,7 +242,7 @@ public GPUAcceleratedHSVPipe(PBOMode pboMode) { // index for the generic position input) gl.glBindAttribLocation(programId, 0, "position"); - // Compile and setup our two shaders with our program + // Compile and set up our two shaders with our program final int vertexId = createShader(gl, programId, k_vertexShader, GL_VERTEX_SHADER); final int fragmentId = createShader(gl, programId, k_fragmentShader, GL_FRAGMENT_SHADER); @@ -298,7 +298,10 @@ public GPUAcceleratedHSVPipe(PBOMode pboMode) { FloatBuffer vertexBuffer = GLBuffers.newDirectFloatBuffer(k_vertexPositions); gl.glBindBuffer(GL_ARRAY_BUFFER, vertexVBOIds.get(0)); gl.glBufferData( - GL_ARRAY_BUFFER, vertexBuffer.capacity() * Float.BYTES, vertexBuffer, GL_STATIC_DRAW); + GL_ARRAY_BUFFER, + (long) vertexBuffer.capacity() * Float.BYTES, + vertexBuffer, + GL_STATIC_DRAW); // Set up pixel unpack buffer (a PBO to transfer image data to the GPU) if (pboMode != PBOMode.NONE) { @@ -389,12 +392,18 @@ protected Mat process(Mat in) { if (pboMode != PBOMode.NONE) { gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(0)); gl.glBufferData( - GLES3.GL_PIXEL_PACK_BUFFER, in.width() * in.height(), null, GLES3.GL_STREAM_READ); + GLES3.GL_PIXEL_PACK_BUFFER, + (long) in.width() * in.height(), + null, + GLES3.GL_STREAM_READ); if (pboMode == PBOMode.DOUBLE_BUFFERED) { gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(1)); gl.glBufferData( - GLES3.GL_PIXEL_PACK_BUFFER, in.width() * in.height(), null, GLES3.GL_STREAM_READ); + GLES3.GL_PIXEL_PACK_BUFFER, + (long) in.width() * in.height(), + null, + GLES3.GL_STREAM_READ); } } } @@ -459,14 +468,17 @@ protected Mat process(Mat in) { // GPU // This causes the previous data in the PBO to be discarded gl.glBufferData( - GLES3.GL_PIXEL_UNPACK_BUFFER, in.width() * in.height() * 3, null, GLES3.GL_STREAM_DRAW); + GLES3.GL_PIXEL_UNPACK_BUFFER, + (long) in.width() * in.height() * 3, + null, + GLES3.GL_STREAM_DRAW); - // Map the a buffer of GPU memory into a place that's accessible by us + // Map the buffer of GPU memory into a place that's accessible by us var buf = gl.glMapBufferRange( GLES3.GL_PIXEL_UNPACK_BUFFER, 0, - in.width() * in.height() * 3, + (long) in.width() * in.height() * 3, GLES3.GL_MAP_WRITE_BIT); buf.put(inputBytes); @@ -527,7 +539,8 @@ private Mat saveMatPBO(GLES3 gl, int width, int height, boolean doubleBuffered) // Map the PBO into the CPU's memory gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(packNextIndex)); var buf = - gl.glMapBufferRange(GLES3.GL_PIXEL_PACK_BUFFER, 0, width * height, GLES3.GL_MAP_READ_BIT); + gl.glMapBufferRange( + GLES3.GL_PIXEL_PACK_BUFFER, 0, (long) width * height, GLES3.GL_MAP_READ_BIT); buf.get(outputBytes); outputMat.put(0, 0, outputBytes); gl.glUnmapBuffer(GLES3.GL_PIXEL_PACK_BUFFER); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java index 162c975d16..e0ba36ce0e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java @@ -40,7 +40,6 @@ protected Mat process(Mat in) { Scalar firstLower = params.getHsvLower().clone(); Scalar firstUpper = params.getHsvUpper().clone(); firstLower.val[0] = params.getHsvUpper().val[0]; - ; firstUpper.val[0] = 180; var lowerThresholdMat = new Mat(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java index c1f6f57126..7bdc9bf0d6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java @@ -37,7 +37,7 @@ protected List process(List in) { } m_sortedContours.clear(); - if (in.size() > 0) { + if (!in.isEmpty()) { m_sortedContours.addAll(in); if (params.getSortMode() != ContourSortMode.Centermost) { m_sortedContours.sort(params.getSortMode().getComparator()); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SpeckleRejectPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SpeckleRejectPipe.java index 2349eef735..4f4c5ce547 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SpeckleRejectPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SpeckleRejectPipe.java @@ -33,7 +33,7 @@ protected List process(List in) { } m_despeckledContours.clear(); - if (in.size() > 0) { + if (!in.isEmpty()) { double averageArea = 0.0; for (Contour c : in) { averageArea += c.getArea(); 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 4e773605f4..af1c5145d4 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 @@ -29,13 +29,15 @@ import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; -import org.photonvision.vision.pipe.impl.*; +import org.photonvision.vision.pipe.impl.AprilTagDetectionPipe; +import org.photonvision.vision.pipe.impl.AprilTagDetectionPipeParams; +import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe; import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams; +import org.photonvision.vision.pipe.impl.CalculateFPSPipe; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; -@SuppressWarnings("DuplicatedCode") public class AprilTagPipeline extends CVPipeline { private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe(); private final AprilTagPoseEstimatorPipe poseEstimatorPipe = new AprilTagPoseEstimatorPipe(); 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 69ca5f9341..26a5c60151 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 @@ -48,7 +48,6 @@ import org.photonvision.vision.target.TrackedTarget; import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; -@SuppressWarnings("DuplicatedCode") public class ArucoPipeline extends CVPipeline { private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); private final GrayscalePipe grayscalePipe = new GrayscalePipe(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java index b89ef0a38c..6ead92a0e8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java @@ -41,7 +41,7 @@ public class CVPipelineSettings implements Cloneable { public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0; public String pipelineNickname = "New Pipeline"; public boolean cameraAutoExposure = false; - // manual exposure only used if cameraAutoExposure if false + // manual exposure only used if cameraAutoExposure is false public double cameraExposure = 20; public int cameraBrightness = 50; // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index fecb36d071..261b8fd522 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -46,7 +46,7 @@ public class Calibrate3dPipeline extends CVPipeline { - // For loggging + // For logging private static final Logger logger = new Logger(Calibrate3dPipeline.class, LogGroup.General); // Only 2 pipes needed, one for finding the board corners and one for actually calibrating @@ -63,7 +63,7 @@ public class Calibrate3dPipeline /// Output of the calibration, getter method is set for this. private CVPipeResult calibrationOutput; - private int minSnapshots; + private final int minSnapshots; private boolean calibrating = false; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index df108d7b2f..190de9e68b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -24,14 +24,16 @@ import org.opencv.core.Point; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; -import org.photonvision.vision.opencv.*; +import org.photonvision.vision.opencv.CVShape; +import org.photonvision.vision.opencv.Contour; +import org.photonvision.vision.opencv.ContourShape; +import org.photonvision.vision.opencv.DualOffsetValues; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; import org.photonvision.vision.pipe.impl.*; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TrackedTarget; -@SuppressWarnings({"DuplicatedCode"}) public class ColoredShapePipeline extends CVPipeline { private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe(); 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 666ecd8ce2..0474b6b562 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 @@ -150,7 +150,7 @@ public CVPipelineResult process( if (!(settings instanceof AprilTagPipelineSettings) && !(settings instanceof ArucoPipelineSettings)) { - // If we're processing anything other than Apriltags.. + // If we're processing anything other than Apriltags... var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw)); sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index cea2e31422..e961488b78 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -30,7 +30,6 @@ import org.photonvision.vision.target.TrackedTarget; /** Represents a pipeline for tracking retro-reflective targets. */ -@SuppressWarnings({"DuplicatedCode"}) public class ReflectivePipeline extends CVPipeline { private final FindContoursPipe findContoursPipe = new FindContoursPipe(); private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe(); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 16eb189155..c6d55211cd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -66,7 +66,7 @@ public PipelineManager( this.driverModePipeline.setSettings(driverSettings); - if (userPipelines.size() < 1) addPipeline(PipelineType.Reflective); + if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective); } public PipelineManager(CameraConfiguration config) { @@ -249,9 +249,9 @@ public void setDriverMode(boolean state) { } /** - * Returns whether or not driver mode is active. + * Returns whether driver mode is active. * - * @return Whether or not driver mode is active. + * @return Whether driver mode is active. */ public boolean getDriverMode() { return currentPipelineIndex == DRIVERMODE_INDEX; @@ -263,7 +263,7 @@ public boolean getDriverMode() { /** * Sorts the pipeline list by index, and reassigns their indexes to match the new order.
*
- * I don't like this but I have no other ideas, and it works so + * I don't like this, but I have no other ideas, and it works so */ private void reassignIndexes() { userPipelineSettings.sort(PipelineSettingsIndexComparator); @@ -322,7 +322,7 @@ private CVPipelineSettings createSettingsForType(PipelineType type, String nickn } default: { - logger.error("Got invalid pipeline type: " + type.toString()); + logger.error("Got invalid pipeline type: " + type); return null; } } @@ -384,31 +384,31 @@ public int duplicatePipeline(int index) { private static String createUniqueName( String nickname, List existingSettings) { - String uniqueName = nickname; + StringBuilder uniqueName = new StringBuilder(nickname); while (true) { - String finalUniqueName = uniqueName; // To get around lambda capture + String finalUniqueName = uniqueName.toString(); // To get around lambda capture var conflictingName = existingSettings.stream().anyMatch(it -> it.pipelineNickname.equals(finalUniqueName)); if (!conflictingName) { // If no conflict, we're done - return uniqueName; + return uniqueName.toString(); } else { // Otherwise, we need to add a suffix to the name // If the string doesn't already end in "([0-9]*)", we'll add it // If it does, we'll increment the number in the suffix - if (uniqueName.matches(".*\\([0-9]*\\)")) { + if (uniqueName.toString().matches(".*\\([0-9]*\\)")) { // Because java strings are immutable, we have to do this curstedness // This is like doing "New pipeline (" + 2 + ")" - var parenStart = uniqueName.lastIndexOf('('); + var parenStart = uniqueName.toString().lastIndexOf('('); var parenEnd = uniqueName.length() - 1; var number = Integer.parseInt(uniqueName.substring(parenStart + 1, parenEnd)) + 1; - uniqueName = uniqueName.substring(0, parenStart + 1) + number + ")"; + uniqueName = new StringBuilder(uniqueName.substring(0, parenStart + 1) + number + ")"); } else { - uniqueName += " (1)"; + uniqueName.append(" (1)"); } } } @@ -450,7 +450,7 @@ public void changePipelineType(int newType) { return; } - logger.info("Adding new pipe of type " + type.toString() + " at idx " + idx); + logger.info("Adding new pipe of type " + type + " at idx " + idx); newSettings.pipelineIndex = idx; userPipelineSettings.set(idx, newSettings); setPipelineInternal(idx); 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 bf0ac94427..d246a45b52 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 @@ -20,7 +20,10 @@ import edu.wpi.first.cscore.VideoException; import edu.wpi.first.math.util.Units; import io.javalin.websocket.WsContext; -import java.util.*; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedList; +import java.util.List; import java.util.function.BiConsumer; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -294,7 +297,7 @@ public void setFov(double fov) { var settables = visionSource.getSettables(); logger.trace(() -> "Setting " + settables.getConfiguration().nickname + ") FOV (" + fov + ")"); - // Only set FOV if we have no vendor JSON and we aren't using a PiCAM + // Only set FOV if we have no vendor JSON, and we aren't using a PiCAM if (isVendorCamera()) { logger.info("Cannot set FOV on a vendor device! Ignoring..."); } else { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java index 4ddcd74d5c..d75e62cb7e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java @@ -65,18 +65,15 @@ public List addSources(List visionSources) { addedModules.put(visionSource.getCameraConfiguration().streamIndex, module); } - var sortedModulesList = - addedModules.entrySet().stream() - .sorted(Comparator.comparingInt(Map.Entry::getKey)) // sort by stream index - .map(Map.Entry::getValue) // map to Stream of VisionModule - .collect(Collectors.toList()); // collect in a List - - return sortedModulesList; + return addedModules.entrySet().stream() + .sorted(Comparator.comparingInt(Map.Entry::getKey)) // sort by stream index + .map(Map.Entry::getValue) // map to Stream of VisionModule + .collect(Collectors.toList()); // collect in a List } private void assignCameraIndex(List config) { - // We won't necessarily have already added all of the cameras we need to at this point - // But by operating on the list, we have a fairly good idea of which we need to change + // We won't necessarily have already added all the cameras we need to at this point + // But by operating on the list, we have a fairly good idea of which we need to change, // but it's not guaranteed that we change the correct one // The best we can do is try to avoid a case where the stream index runs away to infinity // since we can only stream 5 cameras at once 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 bcb75e7f32..4b31ba0691 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 @@ -19,7 +19,10 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.UsbCameraInfo; -import java.util.*; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.List; import java.util.concurrent.CopyOnWriteArrayList; import java.util.function.Supplier; import java.util.stream.Collectors; diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java index 2079abc560..60b278aae4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java @@ -33,7 +33,6 @@ public static double calculatePitch( return -Math.toDegrees(Math.atan((offsetCenterY - targetCenterY) / verticalFocalLength)); } - @SuppressWarnings("DuplicatedCode") public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) { // https://namkeenman.wordpress.com/2015/12/18/open-cv-determine-angle-of-rotatedrect-minarearect/ var angle = minAreaRect.angle; 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 4ead2f7784..ea06638af8 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 @@ -127,8 +127,8 @@ public enum TargetModel implements Releasable { Units.inchesToMeters(3 * 2)); @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates; - @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); - @JsonIgnore private MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); + @JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); + @JsonIgnore private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); @JsonProperty("realWorldCoordinatesArray") private List realWorldCoordinatesArray; diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 7b5bf41131..b0b89273b7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -24,17 +24,14 @@ import edu.wpi.first.math.geometry.Translation3d; import java.util.HashMap; import java.util.List; -import org.opencv.core.CvType; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.opencv.core.Rect2d; -import org.opencv.core.RotatedRect; +import org.opencv.core.*; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.FrameStaticProperties; -import org.photonvision.vision.opencv.*; +import org.photonvision.vision.opencv.CVShape; +import org.photonvision.vision.opencv.Contour; +import org.photonvision.vision.opencv.DualOffsetValues; +import org.photonvision.vision.opencv.Releasable; public class TrackedTarget implements Releasable { public final Contour m_mainContour; @@ -127,11 +124,9 @@ public TrackedTarget( tvec.put( 0, 0, - new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() - }); + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ()); setCameraRelativeTvec(tvec); // Opencv expects a 3d vector with norm = angle and direction = axis @@ -186,10 +181,9 @@ public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters pa var axisangle = VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]); Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF()); - Transform3d targetPose = - MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation)); - m_bestCameraToTarget3d = targetPose; + m_bestCameraToTarget3d = + MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation)); } } @@ -241,7 +235,7 @@ public double getPoseAmbiguity() { } /** - * Set the approximate bouding polygon. + * Set the approximate bounding polygon. * * @param boundingPolygon List of points to copy. Not modified. */ @@ -294,7 +288,7 @@ public void calculateValues(TargetCalculationParameters params) { params.dualOffsetValues, params.robotOffsetPointMode); - // order of this stuff doesnt matter though + // order of this stuff doesn't matter though m_pitch = TargetCalculations.calculatePitch( m_targetOffsetPoint.y, m_robotOffsetPoint.y, params.verticalFocalLength); 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 index 3a9eed25f1..dae270e9f6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java +++ b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java @@ -35,7 +35,7 @@ public class SocketVideoStream implements Consumer { // 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 differeing rates of putting frames in + // Should eliminate synchronization issues of differing rates of putting frames in // and taking them back out boolean frameWasConsumed = false; @@ -53,8 +53,7 @@ public SocketVideoStream(int portID) { this.portID = portID; oldSchoolServer = new MJPGFrameConsumer( - CameraServerJNI.getHostname() + "_Port_" + Integer.toString(portID) + "_MJPEG_Server", - portID); + CameraServerJNI.getHostname() + "_Port_" + portID + "_MJPEG_Server", portID); } @Override @@ -64,7 +63,7 @@ public void accept(CVMat image) { .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 recieve and convert to JPEG for efficency + // 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; 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 index 258c8805ff..ceb84feab6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java @@ -29,8 +29,8 @@ public class SocketVideoStreamManager { private final Logger logger = new Logger(SocketVideoStreamManager.class, LogGroup.Camera); - private Map streams = new Hashtable(); - private Map userSubscriptions = new Hashtable(); + private final Map streams = new Hashtable<>(); + private final Map userSubscriptions = new Hashtable<>(); private static class ThreadSafeSingleton { private static final SocketVideoStreamManager INSTANCE = new SocketVideoStreamManager(); @@ -45,13 +45,13 @@ 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 " + Integer.toString(newStream.portID)); + 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 " + Integer.toString(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 @@ -61,8 +61,7 @@ public void addSubscription(WsContext user, int streamPortID) { userSubscriptions.put(user, streamPortID); stream.addUser(); } else { - logger.error( - "User attempted to subscribe to non-existent port " + Integer.toString(streamPortID)); + logger.error("User attempted to subscribe to non-existent port " + streamPortID); } } @@ -92,7 +91,7 @@ public ByteBuffer getSendFrame(WsContext user) { } } - // Causes all streams to "re-trigger" and recieve and convert their next mjpeg frame + // 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()) { diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java index 8aa1456002..beaae0e4d9 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java @@ -107,13 +107,13 @@ public void deserializeConfig() { Assertions.assertTrue( reflectivePipelineSettings instanceof ReflectivePipelineSettings, - "Conig loaded pipeline settings for index 0 not of expected type ReflectivePipelineSettings!"); + "Config loaded pipeline settings for index 0 not of expected type ReflectivePipelineSettings!"); Assertions.assertTrue( coloredShapePipelineSettings instanceof ColoredShapePipelineSettings, - "Conig loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!"); + "Config loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!"); Assertions.assertTrue( apriltagPipelineSettings instanceof AprilTagPipelineSettings, - "Conig loaded pipeline settings for index 2 not of expected type AprilTagPipelineSettings!"); + "Config loaded pipeline settings for index 2 not of expected type AprilTagPipelineSettings!"); } @AfterAll diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java index ad2567bc3f..18d4ae5b6a 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java @@ -21,7 +21,8 @@ import java.nio.file.Path; import java.util.List; -import org.junit.jupiter.api.*; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.camera.CameraType; import org.photonvision.vision.pipeline.AprilTagPipelineSettings; diff --git a/photon-core/src/test/java/org/photonvision/common/util/LogFileManagementTest.java b/photon-core/src/test/java/org/photonvision/common/util/LogFileManagementTest.java index bfd36a6aea..e834874641 100644 --- a/photon-core/src/test/java/org/photonvision/common/util/LogFileManagementTest.java +++ b/photon-core/src/test/java/org/photonvision/common/util/LogFileManagementTest.java @@ -57,17 +57,15 @@ public void fileCleanupTest() { } // Confirm new log files were created - Assertions.assertEquals( - true, - Logger.MAX_LOGS_TO_KEEP + 5 <= countLogFiles(testDir), - "Not enough log files discovered"); + Assertions.assertTrue( + Logger.MAX_LOGS_TO_KEEP + 5 <= countLogFiles(testDir), "Not enough log files discovered"); // Run the log cleanup routine Logger.cleanLogs(Path.of(testDir)); // Confirm we deleted log files Assertions.assertEquals( - true, Logger.MAX_LOGS_TO_KEEP == countLogFiles(testDir), "Not enough log files deleted"); + Logger.MAX_LOGS_TO_KEEP, countLogFiles(testDir), "Not enough log files deleted"); // Clean uptest directory org.photonvision.common.util.file.FileUtils.deleteDirectory(Path.of(testDir)); diff --git a/photon-core/src/test/java/org/photonvision/hardware/HardwareTest.java b/photon-core/src/test/java/org/photonvision/hardware/HardwareTest.java index 8f6924d755..9d371d25f8 100644 --- a/photon-core/src/test/java/org/photonvision/hardware/HardwareTest.java +++ b/photon-core/src/test/java/org/photonvision/hardware/HardwareTest.java @@ -17,7 +17,8 @@ package org.photonvision.hardware; -import static org.junit.jupiter.api.Assertions.*; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; import org.photonvision.common.hardware.GPIO.CustomGPIO; 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 23f494428d..1ece69fc55 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 @@ -60,7 +60,7 @@ public void testApriltagFacingCamera() { pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); printTestResults(pipelineResult); } catch (RuntimeException e) { - // For now, will throw coz rotation3d ctor + // For now, will throw because of the Rotation3d ctor return; } @@ -116,7 +116,7 @@ public void testApriltagDistorted() { pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); printTestResults(pipelineResult); } catch (RuntimeException e) { - // For now, will throw coz rotation3d ctor + // For now, will throw because of the Rotation3d ctor return; } 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 06d2fb4f5a..168b7b92e9 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 @@ -59,7 +59,7 @@ public void testApriltagFacingCameraAruco() { pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); printTestResults(pipelineResult); } catch (RuntimeException e) { - // For now, will throw coz rotation3d ctor + // For now, will throw because of the Rotation3d ctor return; } diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index bec81b2c49..72547829f1 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -17,7 +17,8 @@ package org.photonvision.vision.pipeline; -import static org.junit.jupiter.api.Assertions.*; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.util.Units; import java.io.File; @@ -310,7 +311,7 @@ public void calibrateSquaresCommon( assertTrue(centerYErrPct < 10.0); System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); - System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString()); + System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics); System.out.println("Dist Coeffs: " + cal.distCoeffs.toString()); System.out.println("Standard Deviation: " + cal.standardDeviation); System.out.println( diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index 14a51b230e..5a4bc4fcd5 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -17,7 +17,8 @@ package org.photonvision.vision.pipeline; -import static org.junit.jupiter.api.Assertions.*; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; import java.util.stream.Collectors; import org.junit.jupiter.api.BeforeEach; diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index 9e16b6c75d..e0a8376159 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -24,7 +24,9 @@ import java.util.HashMap; import java.util.List; import java.util.stream.Collectors; -import org.junit.jupiter.api.*; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; @@ -169,7 +171,6 @@ public void testMultipleStreamIndex() { Arrays.toString( modules.stream() .map(it -> it.visionSource.getCameraConfiguration().streamIndex) - .collect(Collectors.toList()) .toArray())); var idxs = modules.stream() diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index 29ecd7654d..0dd36d90e9 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -21,8 +21,10 @@ import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import org.opencv.core.*; +import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; +import org.opencv.core.RotatedRect; +import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.numbers.DoubleCouple; @@ -31,8 +33,9 @@ public class TargetCalculationsTest { - private static Size imageSize = new Size(800, 600); - private static Point imageCenterPoint = new Point(imageSize.width / 2, imageSize.height / 2); + private static final Size imageSize = new Size(800, 600); + private static final Point imageCenterPoint = + new Point(imageSize.width / 2, imageSize.height / 2); private static final double diagFOV = Math.toRadians(70.0); private static final FrameStaticProperties props = diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 8f48dedd61..d7952394df 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -50,10 +50,10 @@ dependencies { implementation "com.fasterxml.jackson.core:jackson-core:2.12.4" implementation "com.fasterxml.jackson.core:jackson-databind:2.12.4" - implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:$opencvVersion" - implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-jni:$opencvVersion:$jniPlatform" + implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:$openCVversion" + implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-jni:$openCVversion:$jniPlatform" - implementation "org.ejml:ejml-simple:0.41" + implementation "org.ejml:ejml-simple:0.42" // Junit testImplementation wpilibTools.deps.wpilibJava("cscore") @@ -126,7 +126,9 @@ task generateVendorJson() { if (photonlibFileOutput.exists()) { photonlibFileOutput.delete() } - def read = photonlibFileInput.text.replace('${photon_version}', pubVersion) + def read = photonlibFileInput.text + .replace('${photon_version}', pubVersion) + .replace('${frc_year}', frcYear) photonlibFileOutput.write(read) } diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle index 725d47c0e5..9967b24ee2 100644 --- a/photon-lib/publish.gradle +++ b/photon-lib/publish.gradle @@ -37,8 +37,8 @@ copyAllOutputs.dependsOn outputVersions ext.addTaskToCopyAllOutputs = { task -> copyAllOutputs.dependsOn task - copyAllOutputs.inputs.file task.archivePath - copyAllOutputs.from task.archivePath + copyAllOutputs.inputs.file task.archiveFile + copyAllOutputs.from task.archiveFile } def artifactGroupId = 'org.photonvision' @@ -49,7 +49,7 @@ def javaBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-java_ task cppHeadersZip(type: Zip) { destinationDirectory = outputsFolder archiveBaseName = zipBaseName - classifier = "headers" + archiveClassifier = "headers" from(licenseFile) { into '/' @@ -67,7 +67,7 @@ task cppHeadersZip(type: Zip) { task cppSourcesZip(type: Zip) { destinationDirectory = outputsFolder archiveBaseName = zipBaseName - classifier = "sources" + archiveClassifier = "sources" from(licenseFile) { into '/' @@ -84,12 +84,12 @@ build.dependsOn cppSourcesZip addTaskToCopyAllOutputs(cppSourcesZip) task sourcesJar(type: Jar, dependsOn: classes) { - classifier = 'sources' + archiveClassifier = 'sources' from sourceSets.main.allSource } task javadocJar(type: Jar, dependsOn: javadoc) { - classifier = 'javadoc' + archiveClassifier = 'javadoc' from javadoc.destinationDir } @@ -102,14 +102,14 @@ task outputJar(type: Jar, dependsOn: classes) { task outputSourcesJar(type: Jar, dependsOn: classes) { archiveBaseName = javaBaseName destinationDirectory = outputsFolder - classifier = 'sources' + archiveClassifier = 'sources' from sourceSets.main.allSource } task outputJavadocJar(type: Jar, dependsOn: javadoc) { archiveBaseName = javaBaseName destinationDirectory = outputsFolder - classifier = 'javadoc' + archiveClassifier = 'javadoc' from javadoc.destinationDir } @@ -198,5 +198,10 @@ model { } } +// So I don't actually know the _right_ way to tell gradle that the vendordep json publish requires generation first, so we're doing this +getTasksByName("publishVendorjsonPublicationToMavenLocal", false).each { + it.mustRunAfter generateVendorJson +} + publishToMavenLocal.dependsOn libraryBuild publish.dependsOn libraryBuild diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in index dddc1a0e6b..9b8f63aaa4 100644 --- a/photon-lib/src/generate/photonlib.json.in +++ b/photon-lib/src/generate/photonlib.json.in @@ -2,7 +2,8 @@ "fileName": "photonlib.json", "name": "photonlib", "version": "${photon_version}", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "${frc_year}", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 9282e3bdd5..f25e01eba5 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -337,21 +338,17 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { R = R.t(); // rotation of inverse cv::Mat tvecI = -R * tvec; // translation of inverse - Vectord<3> tv; + Eigen::Matrix tv; tv[0] = +tvecI.at(2, 0); tv[1] = -tvecI.at(0, 0); tv[2] = -tvecI.at(1, 0); - Vectord<3> rv; + Eigen::Matrix rv; rv[0] = +rvec.at(2, 0); rv[1] = -rvec.at(0, 0); rv[2] = +rvec.at(1, 0); return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}), - Rotation3d( - // radian_t{rv[0]}, - // radian_t{rv[1]}, - // radian_t{rv[2]} - rv, radian_t{rv.norm()})); + Rotation3d(rv)); } std::optional PhotonPoseEstimator::MultiTagPnpStrategy( diff --git a/photon-server/build.gradle b/photon-server/build.gradle index b965465e84..92b50a81b8 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -34,7 +34,7 @@ dependencies { implementation project(':photon-core') implementation project(':photon-targeting') - implementation "io.javalin:javalin:5.6.2" + implementation "io.javalin:javalin:$javalinVersion" implementation wpilibTools.deps.wpilibJava("wpiutil") implementation wpilibTools.deps.wpilibJava("wpimath") diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 08a8dca19b..168ecc981e 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -4,7 +4,7 @@ apply from: "${rootDir}/shared/common.gradle" dependencies { implementation wpilibTools.deps.wpilibJava("wpimath") - implementation "org.ejml:ejml-simple:0.41" + implementation "org.ejml:ejml-simple:0.42" // Junit testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index 67e4323b4c..ecedfafb40 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/aimandrange/gradlew b/photonlib-cpp-examples/aimandrange/gradlew index a69d9cb6c2..0ef4c1e860 100755 --- a/photonlib-cpp-examples/aimandrange/gradlew +++ b/photonlib-cpp-examples/aimandrange/gradlew @@ -80,7 +80,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-cpp-examples/aimandrange/settings.gradle b/photonlib-cpp-examples/aimandrange/settings.gradle index e387260182..b9552a3f3c 100644 --- a/photonlib-cpp-examples/aimandrange/settings.gradle +++ b/photonlib-cpp-examples/aimandrange/settings.gradle @@ -7,7 +7,7 @@ pluginManagement { mavenLocal() jcenter() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h index c8a205c295..26518470c9 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h @@ -53,11 +53,11 @@ class Robot : public frc::TimedRobot { // PID constants should be tuned per robot const double LINEAR_P = 0.1; const double LINEAR_D = 0.0; - frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D}; + frc::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D}; const double ANGULAR_P = 0.1; const double ANGULAR_D = 0.0; - frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; + frc::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; // Change this to match the name of your camera photonlib::PhotonCamera camera{"photonvision"}; diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 67e4323b4c..ecedfafb40 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/aimattarget/gradlew b/photonlib-cpp-examples/aimattarget/gradlew index a69d9cb6c2..0ef4c1e860 100755 --- a/photonlib-cpp-examples/aimattarget/gradlew +++ b/photonlib-cpp-examples/aimattarget/gradlew @@ -80,7 +80,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-cpp-examples/aimattarget/settings.gradle b/photonlib-cpp-examples/aimattarget/settings.gradle index 2f39cd03ad..44fbca7512 100644 --- a/photonlib-cpp-examples/aimattarget/settings.gradle +++ b/photonlib-cpp-examples/aimattarget/settings.gradle @@ -7,7 +7,7 @@ pluginManagement { mavenLocal() jcenter() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index 8b5344a26a..a639c34008 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -42,7 +42,7 @@ class Robot : public frc::TimedRobot { // Change this to match the name of your camera photonlib::PhotonCamera camera{"photonvision"}; // PID constants should be tuned per robot - frc2::PIDController controller{.1, 0, 0}; + frc::PIDController controller{.1, 0, 0}; frc::XboxController xboxController{0}; diff --git a/photonlib-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle index 67e4323b4c..ecedfafb40 100644 --- a/photonlib-cpp-examples/apriltagExample/build.gradle +++ b/photonlib-cpp-examples/apriltagExample/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/apriltagExample/gradlew b/photonlib-cpp-examples/apriltagExample/gradlew index a69d9cb6c2..0ef4c1e860 100644 --- a/photonlib-cpp-examples/apriltagExample/gradlew +++ b/photonlib-cpp-examples/apriltagExample/gradlew @@ -80,7 +80,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-cpp-examples/apriltagExample/settings.gradle b/photonlib-cpp-examples/apriltagExample/settings.gradle index 2f39cd03ad..44fbca7512 100644 --- a/photonlib-cpp-examples/apriltagExample/settings.gradle +++ b/photonlib-cpp-examples/apriltagExample/settings.gradle @@ -7,7 +7,7 @@ pluginManagement { mavenLocal() jcenter() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h index 2799c703d0..903256cbed 100644 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h @@ -109,8 +109,8 @@ class Drivetrain { frc::Encoder m_leftEncoder{0, 1}; frc::Encoder m_rightEncoder{2, 3}; - frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0}; - frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0}; + frc::PIDController m_leftPIDController{8.5, 0.0, 0.0}; + frc::PIDController m_rightPIDController{8.5, 0.0, 0.0}; frc::AnalogGyro m_gyro{0}; diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index cb4661a3a9..0d262a1504 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -28,10 +28,3 @@ spotless { } apply from: "examples.gradle" - -// Task that depends on the build task for every example -task buildAllExamples { task -> - exampleFolderNames.each { line -> - task.dependsOn(line + ":build") - } -} diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle index 67e4323b4c..ecedfafb40 100644 --- a/photonlib-cpp-examples/getinrange/build.gradle +++ b/photonlib-cpp-examples/getinrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/getinrange/gradlew b/photonlib-cpp-examples/getinrange/gradlew index a69d9cb6c2..0ef4c1e860 100755 --- a/photonlib-cpp-examples/getinrange/gradlew +++ b/photonlib-cpp-examples/getinrange/gradlew @@ -80,7 +80,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-cpp-examples/getinrange/settings.gradle b/photonlib-cpp-examples/getinrange/settings.gradle index 8301f6020c..d8802bb464 100644 --- a/photonlib-cpp-examples/getinrange/settings.gradle +++ b/photonlib-cpp-examples/getinrange/settings.gradle @@ -7,7 +7,7 @@ pluginManagement { mavenLocal() jcenter() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h b/photonlib-cpp-examples/getinrange/src/main/include/Robot.h index a67f2122ed..08dec61134 100644 --- a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/getinrange/src/main/include/Robot.h @@ -53,7 +53,7 @@ class Robot : public frc::TimedRobot { // PID constants should be tuned per robot const double P_GAIN = 0.1; const double D_GAIN = 0.0; - frc2::PIDController controller{P_GAIN, 0.0, D_GAIN}; + frc::PIDController controller{P_GAIN, 0.0, D_GAIN}; // Change this to match the name of your camera photonlib::PhotonCamera camera{"photonvision"}; diff --git a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties index ae04661ee7..7f959b087a 100644 --- a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/photonlib-cpp-examples/gradlew b/photonlib-cpp-examples/gradlew index 8bedea1ac8..89a57c4dfe 100755 --- a/photonlib-cpp-examples/gradlew +++ b/photonlib-cpp-examples/gradlew @@ -36,7 +36,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index fa1a30798c..7d6e58895a 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/aimandrange/settings.gradle b/photonlib-java-examples/aimandrange/settings.gradle index df32b747bf..c48c299f78 100644 --- a/photonlib-java-examples/aimandrange/settings.gradle +++ b/photonlib-java-examples/aimandrange/settings.gradle @@ -6,7 +6,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index 9feeaeaa44..d98e56b952 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/aimattarget/settings.gradle b/photonlib-java-examples/aimattarget/settings.gradle index 67499c9943..df10542440 100644 --- a/photonlib-java-examples/aimattarget/settings.gradle +++ b/photonlib-java-examples/aimattarget/settings.gradle @@ -6,7 +6,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 29a90b45f9..35f533e690 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -28,10 +28,3 @@ spotless { targetExclude("photon-lib/src/main/java/org/photonvision/PhotonVersion.java") } } - -// Task that depends on the build task for every example -task buildAllExamples { task -> - exampleFolderNames.each { line -> - task.dependsOn(line + ":build") - } -} diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle index fa1a30798c..7d6e58895a 100644 --- a/photonlib-java-examples/getinrange/build.gradle +++ b/photonlib-java-examples/getinrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/getinrange/settings.gradle b/photonlib-java-examples/getinrange/settings.gradle index f95cb46e53..cfa00c4e15 100644 --- a/photonlib-java-examples/getinrange/settings.gradle +++ b/photonlib-java-examples/getinrange/settings.gradle @@ -6,7 +6,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties index ae04661ee7..7f959b087a 100644 --- a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/photonlib-java-examples/gradlew b/photonlib-java-examples/gradlew index 8bedea1ac8..89a57c4dfe 100755 --- a/photonlib-java-examples/gradlew +++ b/photonlib-java-examples/gradlew @@ -36,7 +36,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle index fa1a30798c..7d6e58895a 100644 --- a/photonlib-java-examples/simaimandrange/build.gradle +++ b/photonlib-java-examples/simaimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/simaimandrange/settings.gradle b/photonlib-java-examples/simaimandrange/settings.gradle index bedc34e6ab..464d86e81f 100644 --- a/photonlib-java-examples/simaimandrange/settings.gradle +++ b/photonlib-java-examples/simaimandrange/settings.gradle @@ -6,7 +6,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000000..8e0f41cd57 --- /dev/null +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java @@ -0,0 +1,91 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +public class Constants { + // ---------- Vision + // Constants about how your camera is mounted to the robot + public static final double CAMERA_PITCH_RADIANS = + Units.degreesToRadians(15); // Angle "up" from horizontal + public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor + + // How far from the target we want to be + public static final double GOAL_RANGE_METERS = Units.feetToMeters(10); + + // Where the 2020 High goal target is located on the field + // See + // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system + // and https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf + // (pages 4 and 5) + public static final Pose3d TARGET_POSE = + new Pose3d( + new Translation3d( + Units.feetToMeters(52.46), + Units.inchesToMeters(94.66), + Units.inchesToMeters(89.69)), // (center of vision target) + new Rotation3d(0.0, 0.0, Math.PI)); + // ---------- + + // ---------- Drivetrain + public static final int LEFT_MOTOR_CHANNEL = 0; + public static final int RIGHT_MOTOR_CHANNEL = 1; + + // PID constants should be tuned per robot + public static final double LINEAR_P = 0.5; + public static final double LINEAR_I = 0; + public static final double LINEAR_D = 0.1; + + public static final double ANGULAR_P = 0.03; + public static final double ANGULAR_I = 0; + public static final double ANGULAR_D = 0.003; + + // Ratio to multiply joystick inputs by + public static final double DRIVESPEED = 0.75; + + // The following properties are necessary for simulation: + + // Distance from drivetrain left wheels to right wheels + public static final double TRACKWIDTH_METERS = Units.feetToMeters(2.0); + public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(6.0); + + // The motors used in the gearbox for one drivetrain side + public static final DCMotor DRIVE_MOTORS = DCMotor.getCIM(2); + // The gearbox reduction, or how many motor rotations per wheel rotation + public static final double GEARING = 8.0; + + // The drivetrain feedforward values + public static final double LINEAR_KV = 2.0; + public static final double LINEAR_KA = 0.5; + + public static final double ANGULAR_KV = 2.25; + public static final double ANGULAR_KA = 0.3; + // ---------- +} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java index 53197dd216..4f22aa287c 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java @@ -24,6 +24,8 @@ package frc.robot; +import static frc.robot.Constants.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; @@ -31,6 +33,7 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; import frc.robot.sim.DrivetrainSim; +import frc.robot.sim.VisionSim; import org.photonvision.PhotonCamera; import org.photonvision.PhotonUtils; @@ -41,34 +44,18 @@ * project. */ public class Robot extends TimedRobot { - // 2020 High goal target height above ground - public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19); - - // Constants about how your camera is mounted to the robot - public static final double CAMERA_PITCH_RADIANS = - Units.degreesToRadians(15); // Angle "up" from horizontal - public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor - - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(10); - // Change this to match the name of your camera PhotonCamera camera = new PhotonCamera("photonvision"); // PID constants should be tuned per robot - final double LINEAR_P = 0.5; - final double LINEAR_D = 0.1; PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); - - final double ANGULAR_P = 0.03; - final double ANGULAR_D = 0.003; PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); XboxController xboxController = new XboxController(0); // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(0); - PWMVictorSPX rightMotor = new PWMVictorSPX(1); + PWMVictorSPX leftMotor = new PWMVictorSPX(LEFT_MOTOR_CHANNEL); + PWMVictorSPX rightMotor = new PWMVictorSPX(RIGHT_MOTOR_CHANNEL); DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); @Override @@ -92,7 +79,7 @@ public void teleopPeriodic() { double range = PhotonUtils.calculateDistanceToTargetMeters( CAMERA_HEIGHT_METERS, - TARGET_HEIGHT_METERS, + TARGET_POSE.getZ(), CAMERA_PITCH_RADIANS, Units.degreesToRadians(result.getBestTarget().getPitch())); @@ -110,8 +97,8 @@ public void teleopPeriodic() { } } else { // Manual Driver Mode - forwardSpeed = -xboxController.getLeftY() * 0.75; - rotationSpeed = -xboxController.getRightX() * 0.75; + forwardSpeed = -xboxController.getLeftY() * DRIVESPEED; + rotationSpeed = -xboxController.getRightX() * DRIVESPEED; } // Use our forward/turn speeds to control the drivetrain @@ -122,14 +109,17 @@ public void teleopPeriodic() { // Simulation support DrivetrainSim dtSim; + VisionSim visionSim; @Override public void simulationInit() { - dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera); + dtSim = new DrivetrainSim(leftMotor, rightMotor); + visionSim = new VisionSim("main", camera); } @Override public void simulationPeriodic() { dtSim.update(); + visionSim.update(dtSim.getPose()); } } diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java index 0034ac8772..2a9c532e84 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java @@ -24,119 +24,49 @@ package frc.robot.sim; +import static frc.robot.Constants.*; + import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.PWMSim; import frc.robot.Robot; -import java.util.List; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; /** - * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated - * PhotonVision system and one vision target. + * This class handles the simulation of robot physics, sensors, and motor controllers. * *

This class and its methods are only relevant during simulation. While on the real robot, the * real motors/sensors/physics are used instead. */ public class DrivetrainSim { - // Simulated Motor Controllers - PWMSim leftLeader; - PWMSim rightLeader; - - // Simulation Physics - // Configure these to match your drivetrain's physical dimensions - // and characterization results. - double trackwidthMeters = Units.feetToMeters(2.0); - LinearSystem drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem(2.0, 0.5, 2.25, 0.3, trackwidthMeters); - DifferentialDrivetrainSim drivetrainSimulator = + // ----- Simulation specific constants + // Drivetrain plant and simulation. This will calculate how the robot pose changes over time as + // motor voltages are applied. + private static final LinearSystem drivetrainSystem = + LinearSystemId.identifyDrivetrainSystem( + LINEAR_KV, LINEAR_KA, ANGULAR_KV, ANGULAR_KA, TRACKWIDTH_METERS); + private static final DifferentialDrivetrainSim drivetrainSimulator = new DifferentialDrivetrainSim( drivetrainSystem, - DCMotor.getCIM(2), - 8, - trackwidthMeters, - Units.inchesToMeters(6.0 / 2.0), + DRIVE_MOTORS, + GEARING, + TRACKWIDTH_METERS, + WHEEL_DIAMETER_METERS / 2.0, null); + // ----- - // Simulated Vision System. - // Configure these to match your PhotonVision Camera, - // pipeline, and LED setup. - double camDiagFOV = 100.0; // degrees - double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees - double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters - double minTargetArea = 0.1; // percentage (0 - 100) - double maxLEDRange = 20; // meters - int camResolutionWidth = 640; // pixels - int camResolutionHeight = 480; // pixels - PhotonCameraSim cameraSim; - - VisionSystemSim visionSim = new VisionSystemSim("main"); - - // See - // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf - // page 208 - TargetModel targetModel = - new TargetModel( - List.of( - new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)), - new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)), - new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)), - new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5)))); - // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf - // pages 4 and 5 - double tgtXPos = Units.feetToMeters(54); - double tgtYPos = - Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0); - Pose3d farTargetPose = - new Pose3d( - new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS), - new Rotation3d(0.0, 0.0, Math.PI)); - - public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) { - leftLeader = new PWMSim(leftMotorPort); - rightLeader = new PWMSim(rightMotorPort); + // PWM handles for getting commanded motor controller speeds + private final PWMSim leftLeader; + private final PWMSim rightLeader; - // Make the vision target visible to this simulated field. - var visionTarget = new VisionTargetSim(farTargetPose, targetModel); - visionSim.addVisionTargets(visionTarget); - - // Create simulated camera properties. These can be set to mimic your actual camera. - var cameraProp = new SimCameraProperties(); - cameraProp.setCalibration( - camResolutionWidth, camResolutionHeight, Rotation2d.fromDegrees(camDiagFOV)); - cameraProp.setCalibError(0.2, 0.05); - cameraProp.setFPS(25); - cameraProp.setAvgLatencyMs(30); - cameraProp.setLatencyStdDevMs(4); - // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible - // targets. - cameraSim = new PhotonCameraSim(camera, cameraProp, minTargetArea, maxLEDRange); - - // Add the simulated camera to view the targets on this simulated field. - visionSim.addCamera( - cameraSim, - new Transform3d( - new Translation3d(0.25, 0, Robot.CAMERA_HEIGHT_METERS), - new Rotation3d(0, -Robot.CAMERA_PITCH_RADIANS, 0))); - - cameraSim.enableDrawWireframe(true); + public DrivetrainSim(PWMMotorController leftController, PWMMotorController rightController) { + leftLeader = new PWMSim(leftController); + rightLeader = new PWMSim(rightController); } /** @@ -155,20 +85,21 @@ public void update() { drivetrainSimulator.setInputs( leftMotorCmd * RobotController.getBatteryVoltage(), -rightMotorCmd * RobotController.getBatteryVoltage()); - drivetrainSimulator.update(0.02); - // Update PhotonVision based on our new robot position. - visionSim.update(drivetrainSimulator.getPose()); + drivetrainSimulator.update(Robot.kDefaultPeriod); + } + + public Pose2d getPose() { + return drivetrainSimulator.getPose(); } /** - * Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the - * robot onto a specific spot in the field (IE, at the start of each match). + * Resets the simulation back to a pre-defined pose. Useful to simulate the action of placing the + * robot onto a specific spot in the field (e.g. at the start of each match). * * @param pose */ public void resetPose(Pose2d pose) { drivetrainSimulator.setPose(pose); - visionSim.resetRobotPose(pose); } } diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java new file mode 100644 index 0000000000..0be5357c69 --- /dev/null +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java @@ -0,0 +1,127 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.sim; + +import static frc.robot.Constants.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import java.util.List; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; + +/** + * This class handles the simulation of the camera and vision target on the field. Updating this + * class will update the camera data, reflecting what the simulated camera sees. + * + *

This class and its methods are only relevant during simulation. While on the real robot, the + * real camera data is used instead. + */ +public class VisionSim { + // ----- Simulation specific constants + // 2020 High goal target shape + // See + // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf + // page 208 + private static final TargetModel kTargetModel = + new TargetModel( + List.of( + new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)), + new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)), + new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)), + new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5)))); + + // Simulated camera properties. These can be set to mimic your actual camera. + private static final int kResolutionWidth = 640; // pixels + private static final int kResolutionHeight = 480; // pixels + private static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100.0); // degrees + private static final double kAvgErrorPx = 0.2; + private static final double kErrorStdDevPx = 0.05; + private static final double kFPS = 25; + private static final double kAvgLatencyMs = 30; + private static final double kLatencyStdDevMs = 4; + private static final double kMinTargetArea = 0.1; // percentage (0 - 100) + private static final double kMaxLEDRange = 15; // meters + // ----- + + // A simulated vision system which handles simulated cameras and targets. + private VisionSystemSim visionSim; + // The simulation of our PhotonCamera, which will simulate camera frames and target info. + private PhotonCameraSim cameraSim; + + public VisionSim(String name, PhotonCamera camera) { + visionSim = new VisionSystemSim(name); + // Make the vision target visible to this simulated field. + var visionTarget = new VisionTargetSim(TARGET_POSE, kTargetModel); + visionSim.addVisionTargets(visionTarget); + + // Create simulated camera properties from our defined constants. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(kResolutionWidth, kResolutionHeight, kFOVDiag); + cameraProp.setCalibError(kAvgErrorPx, kErrorStdDevPx); + cameraProp.setFPS(kFPS); + cameraProp.setAvgLatencyMs(kAvgLatencyMs); + cameraProp.setLatencyStdDevMs(kLatencyStdDevMs); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(camera, cameraProp, kMinTargetArea, kMaxLEDRange); + + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera( + cameraSim, + new Transform3d( + new Translation3d(0, 0, CAMERA_HEIGHT_METERS), + new Rotation3d(0, -CAMERA_PITCH_RADIANS, 0))); + + cameraSim.enableDrawWireframe(true); + } + + /** + * Update the simulated camera data based on its new field pose. + * + * @param simRobotPose The pose of the simulated robot + */ + public void update(Pose2d simRobotPose) { + visionSim.update(simRobotPose); + } + + /** + * Resets the simulation back to a pre-defined pose. Useful to simulate the action of placing the + * robot onto a specific spot in the field (e.g. at the start of each match). + * + * @param pose + */ + public void resetPose(Pose2d pose) { + visionSim.resetRobotPose(pose); + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/build.gradle b/photonlib-java-examples/swervedriveposeestsim/build.gradle index 7610d2a2ac..424e9fa54a 100644 --- a/photonlib-java-examples/swervedriveposeestsim/build.gradle +++ b/photonlib-java-examples/swervedriveposeestsim/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties index c23a1b3177..d3fb630184 100644 --- a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/photonlib-java-examples/swervedriveposeestsim/gradlew b/photonlib-java-examples/swervedriveposeestsim/gradlew index a69d9cb6c2..0ef4c1e860 100644 --- a/photonlib-java-examples/swervedriveposeestsim/gradlew +++ b/photonlib-java-examples/swervedriveposeestsim/gradlew @@ -80,7 +80,8 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit APP_NAME="Gradle" APP_BASE_NAME=${0##*/} diff --git a/photonlib-java-examples/swervedriveposeestsim/settings.gradle b/photonlib-java-examples/swervedriveposeestsim/settings.gradle index 48c039ed86..091a37a027 100644 --- a/photonlib-java-examples/swervedriveposeestsim/settings.gradle +++ b/photonlib-java-examples/swervedriveposeestsim/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java index 708d87a5b0..1cea631e93 100644 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java @@ -36,7 +36,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import java.io.IOException; public class Constants { public static class Vision { @@ -46,16 +45,8 @@ public static class Vision { new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); // The layout of the AprilTags on the field - public static final AprilTagFieldLayout kTagLayout; - - static { - try { - kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - } catch (IOException e) { - e.printStackTrace(); - throw new RuntimeException(e); - } - } + public static final AprilTagFieldLayout kTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) diff --git a/shared/common.gradle b/shared/common.gradle index 96cab8d526..e8181507e3 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -36,10 +36,10 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:$opencvVersion" - implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-jni:$opencvVersion:$jniPlatform" + implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:$openCVversion" + implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-jni:$openCVversion:$jniPlatform" - implementation "org.ejml:ejml-simple:0.41" + implementation "org.ejml:ejml-simple:0.42" // test stuff testImplementation("org.junit.jupiter:junit-jupiter:5.8.2") @@ -51,6 +51,7 @@ test { events "passed", "skipped", "failed", "standardOut", "standardError" } workingDir = new File("${rootDir}") + finalizedBy jacocoTestReport } task testHeadless(type: Test) { @@ -67,11 +68,19 @@ task generateJavaDocs(type: Javadoc) { destinationDir = file("${projectDir}/build/docs") } + +jacoco { + toolVersion = "0.8.9" + reportsDirectory = layout.buildDirectory.dir('customJacocoReportDir') +} + jacocoTestReport { -// dependsOn testHeadless // Tests are required to run before generating the report + dependsOn testHeadless reports { - xml.enabled true + xml.required = true + csv.required = false + html.outputLocation = layout.buildDirectory.dir('jacocoHtml') } afterEvaluate { diff --git a/shared/config.gradle b/shared/config.gradle index 401d2b7195..3d5f91b150 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -6,11 +6,13 @@ nativeUtils.withCrossLinuxArm64() // Configure WPI dependencies. nativeUtils.wpi.configureDependencies { + opencvYear = 'frc2024' + googleTestYear = "frc2023" + wpiVersion = wpilibVersion wpimathVersion = wpilibVersion - niLibVersion = "2023.3.0" - opencvYear = 'frc2024' - opencvVersion = "4.8.0-1" + niLibVersion = "2024.1.1" + opencvVersion = openCVversion googleTestVersion = "1.12.1-2" imguiVersion = "1.86-1" } @@ -92,7 +94,7 @@ ext.createComponentZipTasks = { components, names, base, type, project, func -> def task = project.tasks.create(base + "-${key}", type) { description = 'Creates component archive for platform ' + key destinationDirectory = outputsFolder - classifier = key + archiveClassifier = key archiveBaseName = '_M_' + base duplicatesStrategy = 'exclude' @@ -126,7 +128,7 @@ ext.createAllCombined = { list, name, base, type, project -> list.each { if (it.name.endsWith('debug')) return - from project.zipTree(it.archivePath) + from project.zipTree(it.archiveFile) dependsOn it } }