diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 80c51e342e..ff0c3014b4 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -45,10 +45,9 @@ jobs: # fetch-depth: 0 # - name: Fetch tags # run: git fetch --tags --force - # - name: Install Java 17 - # uses: actions/setup-java@v5 + # - uses: actions/setup-java@v5 # with: - # java-version: 17 + # java-version: 21 # distribution: temurin # - name: Install SystemCore Toolchain # run: ./gradlew installSystemCoreToolchain @@ -104,10 +103,9 @@ jobs: fetch-depth: 0 - name: Fetch tags run: git fetch --tags --force - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - name: Install pnpm uses: pnpm/action-setup@v5 @@ -145,10 +143,9 @@ jobs: fetch-depth: 0 - name: Fetch tags run: git fetch --tags --force - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - name: Install pnpm uses: pnpm/action-setup@v5 @@ -198,10 +195,9 @@ jobs: with: fetch-depth: 0 - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin # grab all tags @@ -240,10 +236,9 @@ jobs: - uses: actions/checkout@v6 with: fetch-depth: 0 - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - run: git fetch --tags --force - run: ./gradlew photon-targeting:build photon-lib:build @@ -265,13 +260,13 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/systemcore-cross-ubuntu:2025-24.04 + - container: wpilib/systemcore-cross-ubuntu:2027-24.04 artifact-name: SystemCore build-options: "-Ponlylinuxsystemcore" - - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 + - container: wpilib/raspbian-cross-ubuntu:2027-bookworm-24.04 artifact-name: Raspbian build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bookworm-24.04 + - container: wpilib/aarch64-cross-ubuntu:2027-bookworm-24.04 artifact-name: Aarch64 build-options: "-Ponlylinuxarm64" @@ -347,10 +342,9 @@ jobs: - uses: actions/checkout@v6 with: fetch-depth: 0 - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - name: Install pnpm uses: pnpm/action-setup@v5 @@ -431,15 +425,16 @@ jobs: artifact-name: photonvision-*-winx64.jar - os: macos-latest artifact-name: photonvision-*-macarm64.jar + - os: ubuntu-24.04-arm + artifact-name: photonvision-*-linuxarm64.jar runs-on: ${{ matrix.os }} steps: - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - uses: actions/download-artifact@v8 with: diff --git a/.github/workflows/dependency-submission.yml b/.github/workflows/dependency-submission.yml index 7a7c477cac..cbc211e1d8 100644 --- a/.github/workflows/dependency-submission.yml +++ b/.github/workflows/dependency-submission.yml @@ -13,10 +13,9 @@ jobs: steps: - name: Checkout sources uses: actions/checkout@v6 - - name: Setup Java - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: distribution: 'temurin' - java-version: 17 + java-version: 21 - name: Generate and submit dependency graph uses: gradle/actions/dependency-submission@v5 diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 7477309a5a..6e48b38a7f 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -61,7 +61,7 @@ jobs: fetch-depth: 0 - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - run: | set +e diff --git a/.github/workflows/photon-api-docs.yml b/.github/workflows/photon-api-docs.yml index 071fd354a4..30761d86bc 100644 --- a/.github/workflows/photon-api-docs.yml +++ b/.github/workflows/photon-api-docs.yml @@ -60,10 +60,9 @@ jobs: fetch-depth: 0 - name: Fetch tags run: git fetch --tags --force - - name: Install Java 17 - uses: actions/setup-java@v5 + - uses: actions/setup-java@v5 with: - java-version: 17 + java-version: 21 distribution: temurin - name: Build javadocs/doxygen run: | diff --git a/build.gradle b/build.gradle index b5f8a51e1d..1ae80feeb7 100644 --- a/build.gradle +++ b/build.gradle @@ -4,12 +4,13 @@ plugins { id "cpp" id "com.diffplug.spotless" version "8.1.0" id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0" - id "org.wpilib.GradleRIO" version "2027.0.0-alpha-2" - id 'org.photonvision.tools.WpilibTools' version '2.4.1-photon' - id 'com.google.protobuf' version '0.9.3' apply false + id 'org.wpilib.NativeUtils' version '2027.4.1' apply false + id 'org.wpilib.DeployUtils' version '2027.1.0' apply false + id 'org.photonvision.tools.WpilibTools' version '3.0.0-photon' + id 'com.google.protobuf' version '0.9.5' apply false id 'org.wpilib.GradleJni' version '2027.0.0' id "org.ysb33r.doxygen" version "2.0.0" apply false - id 'com.gradleup.shadow' version '8.3.4' apply false + id 'com.gradleup.shadow' version '9.0.0' apply false id "com.github.node-gradle.node" version "7.0.1" apply false } @@ -33,16 +34,20 @@ ext.allOutputsFolder = file("$project.buildDir/outputs") apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2027.0.0-alpha-3-203-g0001ddc7e" + wpilibVersion = "2027.0.0-alpha-4" wpimathVersion = wpilibVersion openCVYear = "2025" openCVversion = "4.10.0-3" + ejmlVersion = "0.43.1"; + jacksonVersion = "2.15.2"; + quickbufVersion = "1.3.3"; + javalinVersion = "6.7.0" libcameraDriverVersion = "v2026.0.0" rknnVersion = "dev-v2026.0.1-1-g89b2888" rubikVersion = "dev-v2026.0.1-4-g13d6279" - frcYear = "2027_alpha1" - mrcalVersion = "dev-v2026.0.0-1-g239d80e"; + frcYear = "2027_alpha4" + mrcalVersion = "v2027.0.1"; pubVersion = versionString isDev = pubVersion.startsWith("dev") @@ -95,7 +100,7 @@ spotless { } wrapper { - gradleVersion = '8.14.3' + gradleVersion = '9.2.0' } ext.getCurrentArch = { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 28199e0174..0c795facac 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.14.3-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photon-core/build.gradle b/photon-core/build.gradle index aa66fc9671..e3ca10ba68 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -6,7 +6,7 @@ ext.licenseFile = file("$rootDir/LICENSE") ext.externalLicensesFolder = file("$rootDir/ExternalLicenses") apply from: "${rootDir}/shared/common.gradle" -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() +wpilibTools.deps.wpilibVersion = wpilibVersion def nativeConfigName = 'wpilibNatives' configurations { @@ -28,7 +28,7 @@ dependencies { wpilibNatives wpilibTools.deps.wpilib("cscore") wpilibNatives wpilibTools.deps.wpilib("apriltag") wpilibNatives wpilibTools.deps.wpilib("hal") - wpilibNatives wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get()) + wpilibNatives wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion) // These stay as implementation dependencies since they don't have native code that gets packaged implementation 'org.zeroturnaround:zt-zip:1.14' diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index 0307e2fb27..5f0e37ba73 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -35,6 +35,8 @@ import org.photonvision.common.networking.NetworkUtils; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.common.util.file.JacksonUtils; +import org.wpilib.driverstation.Alert; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.networktables.LogMessage; import org.wpilib.networktables.MultiSubscriber; import org.wpilib.networktables.NetworkTable; @@ -43,8 +45,6 @@ import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.networktables.StringSubscriber; import org.wpilib.smartdashboard.SmartDashboard; -import org.wpilib.util.Alert; -import org.wpilib.util.Alert.AlertType; import org.wpilib.vision.apriltag.AprilTagFieldLayout; import org.wpilib.vision.camera.CameraServerJNI; @@ -66,9 +66,9 @@ public class NetworkTablesManager { new MultiSubscriber(ntInstance, new String[] {kRootTableName + "/" + kCoprocTableName + "/"}); // Creating the alert up here since it should be persistent - private final Alert conflictAlert = new Alert("PhotonAlerts", "", AlertType.kWarning); + private final Alert conflictAlert = new Alert("PhotonAlerts", "", Level.MEDIUM); - private final Alert mismatchAlert = new Alert("PhotonAlerts", "", AlertType.kWarning); + private final Alert mismatchAlert = new Alert("PhotonAlerts", "", Level.MEDIUM); public boolean conflictingHostname = false; public String conflictingCameras = ""; diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index e097f2926d..98e223c124 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -36,7 +36,6 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; -import org.photonvision.common.util.TimedTaskManager; import org.wpilib.networktables.IntegerPublisher; import org.wpilib.networktables.IntegerSubscriber; diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/OsImageData.java b/photon-core/src/main/java/org/photonvision/common/hardware/OsImageData.java index 11c0b76ccf..9b2845ef68 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/OsImageData.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/OsImageData.java @@ -35,30 +35,8 @@ public class OsImageData { private static final Logger logger = new Logger(OsImageData.class, LogGroup.General); - private static Path imageVersionFile = Path.of("/opt/photonvision/image-version"); private static Path imageMetadataFile = Path.of("/opt/photonvision/image-version.json"); - /** - * The OS image version string, if available. This is legacy, use {@link ImageMetadata}. - * Deprecated for removal in 2027. - */ - @Deprecated public static final Optional IMAGE_VERSION = getImageVersion(); - - private static Optional getImageVersion() { - if (!imageVersionFile.toFile().exists()) { - logger.warn("Photon cannot locate base OS image version at " + imageVersionFile.toString()); - return Optional.empty(); - } - - try { - return Optional.of(Files.readString(imageVersionFile).strip()); - } catch (IOException e) { - logger.error("Couldn't read image-version file", e); - } - - return Optional.empty(); - } - public static final Optional IMAGE_METADATA = getImageMetadata(); public static record ImageMetadata( diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java index 1edbca86c2..430082a63f 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java @@ -28,6 +28,7 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.util.Units; diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java index 0d4edfbd5e..6ce582a007 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java @@ -21,6 +21,7 @@ import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import java.awt.Color; import java.nio.file.Path; import java.util.Arrays; import java.util.List; @@ -29,6 +30,10 @@ import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Point3; +import org.opencv.core.Scalar; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.util.ColorHelper; import org.wpilib.math.geometry.Pose3d; // Ignore the previous calibration data that was stored in the json file. diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 3a879eea11..fff866e689 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -23,7 +23,6 @@ import org.photonvision.jni.CscoreExtras; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.processes.VisionSourceSettables; -import org.wpilib.networktables.BooleanSubscriber; import org.wpilib.util.PixelFormat; import org.wpilib.util.RawFrame; import org.wpilib.vision.camera.CvSink; @@ -81,7 +80,7 @@ public CapturedFrame getInputMat() { if (m_blockForFrames) { // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) var mat = new CVMat(); - // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since + // This is from wpi::nt::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since // Hal::initialize was called // TODO - under the hood, this incurs an extra copy. We should avoid this, if we // can. @@ -106,7 +105,7 @@ public CapturedFrame getInputMat() { cameraMode.width * 3, PixelFormat.kBGR); - // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since + // This is from wpi::nt::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since // Hal::initialize was called long captureTimeUs = CscoreExtras.grabRawSinkFrameTimeoutLastTime( 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 4c3a128826..5ae3539aaf 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 @@ -209,7 +209,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting camToTag = new Transform3d( camToTag.getTranslation(), - new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation())); + new Rotation3d(0, Math.PI, 0).rotateBy(camToTag.getRotation())); tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 2db7fcbab9..7867f27ed5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -104,7 +104,7 @@ public void release() { } /** - * Get the latency between now (wpi::util::Now) and the time at which the image was captured. + * Get the latency between now (wpi::nt::Now) and the time at which the image was captured. * FOOTGUN: the latency is relative to the time at which this method is called. Waiting to call * this method will change the latency this method returns. */ diff --git a/photon-docs/build.gradle b/photon-docs/build.gradle index 6d80846933..e7ca8ece58 100644 --- a/photon-docs/build.gradle +++ b/photon-docs/build.gradle @@ -119,7 +119,7 @@ task generateJavaDocs(type: Javadoc) { source exportedProjects.collect { project(it).sourceSets.main.allJava } classpath = files(exportedProjects.collect { project(it).sourceSets.main.compileClasspath }) - options.links "https://docs.oracle.com/en/java/javase/17/docs/api/", "https://github.wpilib.org/allwpilib/docs/release/java/" + options.links "https://docs.oracle.com/en/java/javase/21/docs/api/", "https://github.wpilib.org/allwpilib/docs/release/java/" options.addStringOption("tag", "pre:a:Pre-Condition") options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true) options.addBooleanOption('html5', true) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index ce0e4a97b0..82e272cd3d 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -353,7 +353,7 @@ publishing { } // setup wpilib bundled native libs -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() +wpilibTools.deps.wpilibVersion = wpilibVersion def nativeConfigName = 'wpilibNatives' def nativeConfig = configurations.create(nativeConfigName) @@ -375,4 +375,4 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") -nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get()) +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index a8961113cd..653afafa76 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -118,10 +118,7 @@ def __init__(self, cameraName: str): inst.start() # Usage reporting - hal.report( - hal.tResourceType.kResourceType_PhotonCamera.value, - PhotonCamera.instance_count, - ) + hal.reportUsage("PhotonVision/PhotonCamera", PhotonCamera.instance_count, "") PhotonCamera.instance_count += 1 def getAllUnreadResults(self) -> List[PhotonPipelineResult]: diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index e47251360f..f487e5523d 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -66,9 +66,8 @@ def __init__( self._headingBuffer = TimeInterpolatableRotation2dBuffer(1) # Usage reporting - hal.report( - hal.tResourceType.kResourceType_PhotonPoseEstimator.value, - PhotonPoseEstimator.instance_count, + hal.reportUsage( + "PhotonVision/PhotonPoseEstimator", PhotonPoseEstimator.instance_count, "" ) PhotonPoseEstimator.instance_count += 1 diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 61467f6cb3..c16fa36f45 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -33,6 +33,7 @@ import org.photonvision.common.networktables.PacketSubscriber; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.timesync.TimeSyncSingleton; +import org.wpilib.driverstation.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.HAL; import org.wpilib.math.linalg.MatBuilder; @@ -51,8 +52,6 @@ import org.wpilib.networktables.PubSubOption; import org.wpilib.networktables.StringSubscriber; import org.wpilib.system.Timer; -import org.wpilib.util.Alert; -import org.wpilib.util.Alert.AlertType; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { @@ -137,8 +136,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { name = cameraName; disconnectAlert = new Alert( - PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning); - timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning); + PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", Alert.Level.MEDIUM); + timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", Alert.Level.MEDIUM); rootPhotonTable = instance.getTable(kTableName); this.cameraTable = rootPhotonTable.getSubTable(cameraName); path = cameraTable.getPath(); @@ -149,7 +148,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { PhotonPipelineResult.photonStruct.getTypeString(), new byte[0], PubSubOption.periodic(0.01), - PubSubOption.sendAll(true), + PubSubOption.SEND_ALL, PubSubOption.pollStorage(20)); resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); @@ -172,8 +171,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { // Existing is enough to make this multisubscriber do its thing topicNameSubscriber = - new MultiSubscriber( - instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); + new MultiSubscriber(instance, new String[] {"/photonvision/"}, PubSubOption.TOPICS_ONLY); InstanceCount++; HAL.reportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); @@ -576,7 +574,7 @@ else if (!isConnected()) { // spotless:on DriverStation.reportWarning(bfw, false); - var versionMismatchMessage = + String versionMismatchMessage = "Photon version " + PhotonVersion.versionString + " (message definition version " diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 0db4785f48..4a05860eae 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -152,7 +152,7 @@ public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCam this.fieldTags = fieldTags; this.robotToCamera = robotToCamera; - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); + HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); InstanceCount++; } @@ -1140,7 +1140,7 @@ public Optional estimateAverageBestTargetsPose( double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity; Pose3d estimatedPose = pair.getSecond(); transform = transform.plus(estimatedPose.getTranslation().times(weight)); - rotation = rotation.plus(estimatedPose.getRotation().times(weight)); + rotation = rotation.rotateBy(estimatedPose.getRotation().times(weight)); } return Optional.of( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index af9ff94de5..1b7eba56f3 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -531,7 +531,7 @@ public PhotonPipelineResult process( // Simulate confidence using sqrt-scaled area for a more realistic // curve. Raw areaPercent/100 is tiny for most targets; sqrt scaling // gives reasonable values even for small-but-visible objects. - conf = (float) MathUtil.clamp(Math.sqrt(areaPercent / 100.0) * 2.0, 0.0, 1.0); + conf = (float) Math.clamp(Math.sqrt(areaPercent / 100.0) * 2.0, 0.0, 1.0); } detectableTgts.add( diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index abcb24169c..cd69705185 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -29,15 +29,16 @@ #include #include -#include #include #include #include +#include #include #include #include #include #include +#include #include "PhotonVersion.h" #include "photon/dataflow/structures/Packet.h" @@ -164,8 +165,8 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, disconnectAlert(PHOTON_ALERT_GROUP, std::string{"PhotonCamera '"} + std::string{cameraName} + "' is disconnected.", - wpi::Alert::AlertType::kWarning), - timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::AlertType::kWarning) { + wpi::Alert::Level::MEDIUM), + timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::Level::MEDIUM) { verifyDependencies(); InstanceCount++; HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 3dd32a58e0..576eb11c27 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -33,8 +33,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -42,6 +41,7 @@ #include #include #include +#include #include "photon/PhotonCamera.h" #include "photon/estimation/TargetModel.h" @@ -74,8 +74,7 @@ PhotonPoseEstimator::PhotonPoseEstimator( poseCacheTimestamp(-1_s), headingBuffer( wpi::math::TimeInterpolatableBuffer(1_s)) { - HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, - InstanceCount); + HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); InstanceCount++; } @@ -187,10 +186,8 @@ std::optional PhotonPoseEstimator::Update( } break; case CONSTRAINED_SOLVEPNP: { - using namespace frc; - if (!cameraMatrixData || !cameraDistCoeffs) { - WPILib_ReportError( + WPILIB_ReportError( wpi::warn::Warning, "No camera calibration data provided for Constrained SolvePnP!"); ret = Update(result, this->multiTagFallbackStrategy); @@ -255,7 +252,7 @@ std::optional PhotonPoseEstimator::Update( bool ShouldEstimate(const PhotonPipelineResult& result) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { - WPILib_ReportError(wpi::warn::Warning, + WPILIB_ReportError(wpi::warn::Warning, "Result timestamp was reported in the past!"); return false; } @@ -624,7 +621,7 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( pair : tempPoses) { double const weight = (1. / pair.second.first) / totalAmbiguity; transform = transform + pair.first.Translation() * weight; - rotation = rotation + pair.first.Rotation() * weight; + rotation = rotation.RotateBy(pair.first.Rotation() * weight); } return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation), @@ -661,7 +658,6 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose, aprilTags, photon::kAprilTag36h11, headingFree, wpi::math::Rotation2d{ - headingBuffer.Sample(cameraResult.GetTimestamp()).value()}, headingScaleFactor); diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index e3e289eb6f..d3f18b1c64 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -51,7 +51,7 @@ PhotonCameraSim::PhotonCameraSim( videoSimRaw = wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), prop.GetResHeight()); - videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray); + videoSimRaw.SetPixelFormat(wpi::util::PixelFormat::kGray); videoSimProcessed = wpi::CameraServer::PutVideo( std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), prop.GetResHeight()); @@ -89,14 +89,16 @@ bool PhotonCameraSim::CanSeeCorner(const std::vector& points) { return true; } std::optional PhotonCameraSim::ConsumeNextEntryTime() { - uint64_t now = wpi::util::Now(); - uint64_t timestamp{}; + int64_t now = wpi::nt::Now(); + int64_t timestamp{}; + bool hasTimestamp = false; int iter = 0; while (now >= nextNTEntryTime) { timestamp = nextNTEntryTime; - uint64_t frameTime = prop.EstSecUntilNextFrame() - .convert() - .to(); + hasTimestamp = true; + int64_t frameTime = prop.EstSecUntilNextFrame() + .convert() + .to(); nextNTEntryTime += frameTime; if (iter++ > 50) { @@ -106,8 +108,8 @@ std::optional PhotonCameraSim::ConsumeNextEntryTime() { } } - if (timestamp != 0) { - return timestamp; + if (hasTimestamp) { + return static_cast(timestamp); } else { return std::nullopt; } @@ -363,7 +365,7 @@ PhotonPipelineResult PhotonCameraSim::Process( detectableTgts, multiTagResults}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { - SubmitProcessedFrame(result, wpi::util::Now()); + SubmitProcessedFrame(result, wpi::nt::Now()); } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, uint64_t ReceiveTimestamp) { diff --git a/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp b/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp index e1555ce08a..3147452d3d 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include using namespace photon; @@ -75,32 +75,32 @@ void SimCameraProperties::SetCalibration( 1_m, wpi::math::Rotation3d{ 0_rad, 0_rad, - (GetPixelYaw(0) + - wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}}) + (GetPixelYaw(0) + wpi::math::Rotation2d{wpi::units::radian_t{ + -std::numbers::pi / 2.0}}) .Radians()}}, wpi::math::Translation3d{ 1_m, wpi::math::Rotation3d{ 0_rad, 0_rad, - (GetPixelYaw(width) + - wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}}) + (GetPixelYaw(width) + wpi::math::Rotation2d{wpi::units::radian_t{ + std::numbers::pi / 2.0}}) .Radians()}}, wpi::math::Translation3d{ 1_m, wpi::math::Rotation3d{ 0_rad, - (GetPixelPitch(0) + - wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}}) + (GetPixelPitch(0) + wpi::math::Rotation2d{wpi::units::radian_t{ + std::numbers::pi / 2.0}}) .Radians(), 0_rad}}, wpi::math::Translation3d{ 1_m, - wpi::math::Rotation3d{ - 0_rad, - (GetPixelPitch(height) + - wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}}) - .Radians(), - 0_rad}}, + wpi::math::Rotation3d{0_rad, + (GetPixelPitch(height) + + wpi::math::Rotation2d{wpi::units::radian_t{ + -std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, }; viewplanes.clear(); for (size_t i = 0; i < p.size(); i++) { diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index f5442cc81f..1e8a9ca738 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include "photon/targeting/PhotonPipelineResult.h" @@ -204,7 +204,9 @@ class PhotonCamera { */ static void SetVersionCheckEnabled(bool enabled); - std::shared_ptr GetCameraTable() const { return rootTable; } + std::shared_ptr GetCameraTable() const { + return rootTable; + } // For use in tests bool test = false; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 14bf4eca17..d3551927c4 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -27,12 +27,12 @@ #include #include -#include #include #include #include #include #include +#include #include "photon/PhotonCamera.h" #include "photon/targeting/PhotonPipelineResult.h" @@ -94,7 +94,7 @@ class PhotonPoseEstimator { * @param robotToCamera Transform3d from the center of the robot to the camera * mount positions (ie, robot ➔ camera). */ - explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, + explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags, wpi::math::Transform3d robotToCamera); /** @@ -212,9 +212,7 @@ class PhotonPoseEstimator { */ [[deprecated("Use individual estimation methods instead.")]] inline void SetLastPose(wpi::math::Pose3d lastPose) { - this->lastPose = lastPose; - } /** @@ -417,8 +415,8 @@ class PhotonPoseEstimator { std::optional EstimateConstrainedSolvepnpPose( photon::PhotonPipelineResult cameraResult, photon::PhotonCamera::CameraMatrix cameraMatrix, - photon::PhotonCamera::DistortionMatrix distCoeffs, wpi::math::Pose3d seedPose, - bool headingFree, double headingScaleFactor); + photon::PhotonCamera::DistortionMatrix distCoeffs, + wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor); private: wpi::apriltag::AprilTagFieldLayout aprilTags; diff --git a/photon-lib/src/main/native/include/photon/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h index ef87f01a59..dbf816e8ae 100644 --- a/photon-lib/src/main/native/include/photon/PhotonUtils.h +++ b/photon-lib/src/main/native/include/photon/PhotonUtils.h @@ -50,10 +50,9 @@ class PhotonUtils { * values up. * @return The estimated distance to the target. */ - static wpi::units::meter_t CalculateDistanceToTarget(wpi::units::meter_t cameraHeight, - wpi::units::meter_t targetHeight, - wpi::units::radian_t cameraPitch, - wpi::units::radian_t targetPitch) { + static wpi::units::meter_t CalculateDistanceToTarget( + wpi::units::meter_t cameraHeight, wpi::units::meter_t targetHeight, + wpi::units::radian_t cameraPitch, wpi::units::radian_t targetPitch) { return (targetHeight - cameraHeight) / wpi::units::math::tan(cameraPitch + targetPitch); } diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index d19fd4751b..21552e7034 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -88,7 +88,8 @@ class PhotonCameraSim { * separate from this. */ PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, - double minTargetAreaPercent, wpi::units::meter_t maxSightRange); + double minTargetAreaPercent, + wpi::units::meter_t maxSightRange); /** * Returns the camera being simulated. @@ -183,7 +184,9 @@ class PhotonCameraSim { * * @param rangeMeters The distance */ - inline void SetMaxSightRange(wpi::units::meter_t range) { maxSightRange = range; } + inline void SetMaxSightRange(wpi::units::meter_t range) { + maxSightRange = range; + } /** * Sets whether the raw video stream simulation is enabled. @@ -242,7 +245,7 @@ class PhotonCameraSim { NTTopicSet ts{}; int64_t heartbeatCounter{0}; - uint64_t nextNTEntryTime{wpi::util::Now()}; + int64_t nextNTEntryTime{wpi::nt::Now()}; wpi::units::meter_t maxSightRange{std::numeric_limits::max()}; static constexpr double kDefaultMinAreaPx{100}; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index daa0df98b7..2436b1870f 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -290,8 +290,8 @@ class SimCameraProperties { } wpi::math::Rotation2d GetDiagFOV() const { - return wpi::math::Rotation2d{ - wpi::units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + return wpi::math::Rotation2d{wpi::units::math::hypot( + GetHorizFOV().Radians(), GetVertFOV().Radians())}; } /** @@ -353,8 +353,8 @@ class SimCameraProperties { * @return The latency estimate */ wpi::units::second_t EstLatency() { - return wpi::units::math::max(avgLatency + gaussian(generator) * latencyStdDev, - 0_s); + return wpi::units::math::max( + avgLatency + gaussian(generator) * latencyStdDev, 0_s); } /** diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h index fcdf4a40a5..cb0ebad45c 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include "photon/simulation/SimCameraProperties.h" diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 005ec0d0d7..70fd3d6a8b 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -102,7 +102,7 @@ public void testRotConvert() { var rvec = OpenCVHelp.rotationToRvec(rot); var result = OpenCVHelp.rvecToRotation(rvec); rvec.release(); - var diff = result.minus(rot); + var diff = result.relativeTo(rot); assertSame(new Rotation3d(), diff); } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index dfb0f2953a..04cb0364de 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -34,6 +34,7 @@ import java.io.IOException; import java.util.Arrays; import java.util.List; +import java.util.Objects; import java.util.Optional; import java.util.stream.Stream; import org.junit.jupiter.api.AfterEach; @@ -56,6 +57,7 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.networktables.NetworkTablesJNI; +import org.wpilib.simulation.AlertSim; import org.wpilib.simulation.SimHooks; import org.wpilib.smartdashboard.SmartDashboard; import org.wpilib.system.DataLogManager; @@ -322,13 +324,16 @@ public void testAlerts() throws InterruptedException { // WHEN we update the camera camera.getAllUnreadResults(); - // AND we tick SmartDashboard - SmartDashboard.updateValues(); + // TODO: change to getActive once that exists + String[] alertsText = + Arrays.stream(AlertSim.getAll()) + .filter(it -> it.isActive()) + .map(it -> it.text) + .toArray(String[]::new); // The alert state will be set (hard-coded here) assertTrue( - Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) - .anyMatch(it -> it.equals(disconnectedCameraString))); + Arrays.stream(alertsText).anyMatch(it -> Objects.equals(it, disconnectedCameraString))); Thread.sleep(20); } @@ -351,16 +356,18 @@ public void testAlerts() throws InterruptedException { // WHEN we update the camera camera.getAllUnreadResults(); - // AND we tick SmartDashboard - SmartDashboard.updateValues(); + // TODO: change to getActive once that exists + String[] alertsText = + Arrays.stream(AlertSim.getAll()) + .filter(it -> it.isActive()) + .map(it -> it.text) + .toArray(String[]::new); // THEN the camera isn't disconnected - assertTrue( - Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) - .noneMatch(it -> it.equals(disconnectedCameraString))); + assertTrue(Arrays.stream(alertsText).noneMatch(it -> it.equals(disconnectedCameraString))); // AND the alert string looks like a timesync warning assertTrue( - Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) + Arrays.stream(alertsText) .filter(it -> it.contains("is not connected to the TimeSyncServer")) .count() == 1); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index d880cd5bb3..0411302d70 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -49,13 +49,11 @@ import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; import org.wpilib.hardware.hal.HAL; -import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Quaternion; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Transform3d; -import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.VecBuilder; diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 9e3162cde2..0317c84826 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -26,14 +26,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include "gtest/gtest.h" #include "photon/PhotonCamera.h" @@ -48,11 +48,13 @@ #include "photon/targeting/PnpResult.h" WPI_IGNORE_DEPRECATED +namespace units = wpi::units; + static std::vector tags = { - {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - wpi::math::Rotation3d())}, - {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - wpi::math::Rotation3d())}}; + {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), + units::meter_t(3), wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), + units::meter_t(5), wpi::math::Rotation3d())}}; static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; @@ -70,23 +72,23 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -113,10 +115,10 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { std::vector tags = { - {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - wpi::math::Rotation3d())}, - {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - wpi::math::Rotation3d())}, + {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), + units::meter_t(3), wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), + units::meter_t(5), wpi::math::Rotation3d())}, }; auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft); @@ -131,23 +133,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -180,23 +182,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -206,8 +208,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, {}); - estimator.SetReferencePose( - wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); + estimator.SetReferencePose(wpi::math::Pose3d( + 1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -231,23 +233,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -257,8 +259,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, {}); - estimator.SetLastPose( - wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); + estimator.SetLastPose(wpi::math::Pose3d( + 1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -272,23 +274,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.testResult = {photon::PhotonPipelineResult{ @@ -332,8 +334,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); /* real pose of the robot base to test against */ - wpi::math::Pose3d realPose = - wpi::math::Pose3d(7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad)); + wpi::math::Pose3d realPose = wpi::math::Pose3d( + 7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad)); photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), @@ -359,12 +361,12 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { units::unit_cast(pose.Z()), .01); /* Straight on */ - wpi::math::Transform3d straightOnTestTransform = - wpi::math::Transform3d(0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)); + wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d( + 0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)); estimator.SetRobotToCameraTransform(straightOnTestTransform); realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m, - wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad)); + wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), targets); @@ -396,23 +398,23 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -445,23 +447,23 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -520,8 +522,9 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { EXPECT_FALSE(estimatedPose); // Setting ReferencePose should also clear the cache - estimator.SetReferencePose(wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), - units::meter_t(3), wpi::math::Rotation3d())); + estimator.SetReferencePose( + wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), units::meter_t(3), + wpi::math::Rotation3d())); for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); @@ -539,16 +542,16 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}}; cameraOne.test = true; @@ -589,7 +592,8 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( - wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo), + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d()); photon::PhotonPipelineResult result; @@ -613,10 +617,10 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { wpi::math::Transform3d poseTransform( wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m, - 0.48678786477534686_m), - wpi::math::Rotation3d(wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333, - -0.08413452932300695, - 0.9130568172784148))); + 0.48678786477534686_m), + wpi::math::Rotation3d( + wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333, + -0.08413452932300695, 0.9130568172784148))); std::vector targets{ photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform, @@ -635,11 +639,13 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); const units::radian_t camPitch = 30_deg; - const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), - wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; + const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), + wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPoseEstimator estimator( - wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo), + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), photon::CONSTRAINED_SOLVEPNP, kRobotToCam); estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index 0f4b383ce4..b704a3abc4 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include TEST(TimeSyncProtocolTest, Smoketest) { @@ -60,8 +61,6 @@ TEST(TimeSyncProtocolTest, Smoketest) { } TEST(PhotonCameraTest, Alerts) { - using wpi::SmartDashboard; - // GIVEN a local-only NT instance auto inst = wpi::nt::NetworkTableInstance::GetDefault(); inst.StopClient(); @@ -78,20 +77,29 @@ TEST(PhotonCameraTest, Alerts) { std::string disconnectedCameraString = "PhotonCamera '" + cameraName + "' is disconnected."; + // TODO: Replace this when getActive is upstreamed to wpilib + auto getActiveAlerts = []() { + auto infos = wpi::sim::AlertSim::GetAll(); + + std::erase_if(infos, [](const wpi::sim::AlertSim::AlertInfo& info) { + return !info.isActive(); + }); + + return infos; + }; + // Loop to hit cases past first iteration for (int i = 0; i < 10; i++) { // WHEN we update the camera camera.GetAllUnreadResults(); - // AND we tick SmartDashboard - SmartDashboard::UpdateValues(); // The alert state will be set (hard-coded here) - auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {}); - EXPECT_TRUE( - std::any_of(alerts.begin(), alerts.end(), - [&disconnectedCameraString](const std::string& alert) { - return alert == disconnectedCameraString; - })); + auto alerts = getActiveAlerts(); + EXPECT_TRUE(std::any_of(alerts.begin(), alerts.end(), + [&disconnectedCameraString]( + const wpi::sim::AlertSim::AlertInfo& alert) { + return alert.text == disconnectedCameraString; + })); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } @@ -109,25 +117,23 @@ TEST(PhotonCameraTest, Alerts) { sim.SubmitProcessedFrame(noPongResult); // WHEN we update the camera camera.GetAllUnreadResults(); - // AND we tick SmartDashboard - SmartDashboard::UpdateValues(); // THEN the camera isn't disconnected - auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {}); - fmt::println("{}:{}: saw alerts: {}", __FILE__, __LINE__, alerts); - EXPECT_TRUE( - std::none_of(alerts.begin(), alerts.end(), - [&disconnectedCameraString](const std::string& alert) { - return alert == disconnectedCameraString; - })); + auto alerts = getActiveAlerts(); + EXPECT_TRUE(std::none_of(alerts.begin(), alerts.end(), + [&disconnectedCameraString]( + const wpi::sim::AlertSim::AlertInfo& alert) { + return alert.text == disconnectedCameraString; + })); // AND the alert string looks like a timesync warning - EXPECT_EQ( - 1, std::count_if( - alerts.begin(), alerts.end(), [](const std::string& alert) { - return alert.find("is not connected to the TimeSyncServer") != - std::string::npos; - })); + EXPECT_EQ(1, std::count_if( + alerts.begin(), alerts.end(), + [](const wpi::sim::AlertSim::AlertInfo& alert) { + return alert.text.find( + "is not connected to the TimeSyncServer") != + std::string::npos; + })); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index e958ae79db..342d9aa389 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -205,8 +205,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.EstimateClosestToReferencePose( - result, - wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); + result, wpi::math::Pose3d(1_m, 1_m, 1_m, + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); } ASSERT_TRUE(estimatedPose); @@ -493,10 +493,15 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( wpi::apriltag::AprilTagFieldLayout::LoadField( wpi::apriltag::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d()); + wpi::math::Transform3d()); photon::PhotonPipelineResult result; - auto estimate = estimator.Update(result); + auto distortion = Eigen::VectorXd::Zero(8); + auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5}, + {0, 399.16666666666674, 239.5}, + {0, 0, 1}}; + auto estimate = estimator.EstimateConstrainedSolvepnpPose( + result, cameraMat, distortion, wpi::math::Pose3d(), true, 0.0); EXPECT_FALSE(estimate.has_value()); } diff --git a/photon-lib/src/test/native/cpp/VersionTest.cpp b/photon-lib/src/test/native/cpp/VersionTest.cpp index e29430bbb1..442f14adb2 100644 --- a/photon-lib/src/test/native/cpp/VersionTest.cpp +++ b/photon-lib/src/test/native/cpp/VersionTest.cpp @@ -23,10 +23,10 @@ */ #include -#include +#include #include "PhotonVersion.h" TEST(VersionTest, PrintVersion) { - wpi::println("{}", photon::PhotonVersion::versionString); + wpi::util::println("{}", photon::PhotonVersion::versionString); } diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index e68ece5e8e..651e6f1d8b 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -50,13 +50,14 @@ class VisionSystemSimTestWithParamsTest public testing::WithParamInterface {}; class VisionSystemSimTestDistanceParamsTest : public VisionSystemSimTest, - public testing::WithParamInterface< - std::tuple> {}; + public testing::WithParamInterface> {}; TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{15.98_m, 0_m, 2_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; @@ -110,10 +111,11 @@ TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { // Now walk it by yourself visionSysSim.AdjustCamera( - &cameraSim, wpi::math::Transform3d{ - wpi::math::Translation3d{}, - wpi::math::Rotation3d{ - 0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}}); + &cameraSim, + wpi::math::Transform3d{ + wpi::math::Translation3d{}, + wpi::math::Rotation3d{0_deg, 0_deg, + wpi::units::radian_t{std::numbers::pi}}}); visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); } @@ -122,20 +124,22 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) { photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); std::vector targets; for (int i = 0; i < 100; i++) { targets.emplace_back( - frc::Pose3d{ - frc::Translation3d{15.98_m + i * 0.1_m, 0_m, 1_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}, + wpi::math::Pose3d{ + wpi::math::Translation3d{15.98_m + i * 0.1_m, 0_m, 1_m}, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}, photon::TargetModel{0.5_m, 0.5_m}, i); } visionSysSim.AddVisionTargets(targets); - frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_EQ(camera.GetLatestResult().targets.size(), 50u); @@ -144,7 +148,8 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) { TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{15.98_m, 0_m, 1_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; @@ -160,10 +165,11 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { ASSERT_TRUE(camera.GetLatestResult().HasTargets()); visionSysSim.AdjustCamera( - &cameraSim, wpi::math::Transform3d{ - wpi::math::Translation3d{0_m, 0_m, 5000_m}, - wpi::math::Rotation3d{ - 0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}}); + &cameraSim, + wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 5000_m}, + wpi::math::Rotation3d{0_deg, 0_deg, + wpi::units::radian_t{std::numbers::pi}}}); visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); } @@ -171,7 +177,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{15.98_m, 0_m, 2_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; wpi::math::Transform3d robotToCamera{ wpi::math::Translation3d{0_m, 0_m, 1_m}, @@ -200,7 +207,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{15.98_m, 0_m, 1_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; @@ -225,7 +233,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{15.98_m, 0_m, 1_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; @@ -291,10 +300,10 @@ TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { robotPose = wpi::math::Pose2d{wpi::math::Translation2d{10_m, 0_m}, wpi::math::Rotation2d{-1 * GetParam()}}; visionSysSim.AdjustCamera( - &cameraSim, - wpi::math::Transform3d{ - wpi::math::Translation3d{}, - wpi::math::Rotation3d{0_rad, wpi::units::degree_t{GetParam()}, 0_rad}}); + &cameraSim, wpi::math::Transform3d{ + wpi::math::Translation3d{}, + wpi::math::Rotation3d{ + 0_rad, wpi::units::degree_t{GetParam()}, 0_rad}}); visionSysSim.Update(robotPose); const auto result = camera.GetLatestResult(); @@ -370,13 +379,16 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(VisionSystemSimTest, TestMultipleTargets) { wpi::math::Pose3d targetPoseL{ wpi::math::Translation3d{15.98_m, 2_m, 0_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; wpi::math::Pose3d targetPoseC{ wpi::math::Translation3d{15.98_m, 0_m, 0_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; wpi::math::Pose3d targetPoseR{ wpi::math::Translation3d{15.98_m, -2_m, 0_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; @@ -449,20 +461,20 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { std::vector tagList; tagList.emplace_back(wpi::apriltag::AprilTag{ - 0, - wpi::math::Pose3d{12_m, 3_m, 1_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 0, wpi::math::Pose3d{ + 12_m, 3_m, 1_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); tagList.emplace_back(wpi::apriltag::AprilTag{ - 1, - wpi::math::Pose3d{12_m, 1_m, -1_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 1, wpi::math::Pose3d{ + 12_m, 1_m, -1_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); tagList.emplace_back(wpi::apriltag::AprilTag{ - 2, - wpi::math::Pose3d{11_m, 0_m, 2_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 2, wpi::math::Pose3d{ + 11_m, 0_m, 2_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); wpi::units::meter_t fieldLength{54}; wpi::units::meter_t fieldWidth{27}; wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; @@ -488,8 +500,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { ASSERT_NEAR(5, pose.X().to(), 0.01); ASSERT_NEAR(1, pose.Y().to(), 0.01); ASSERT_NEAR(0, pose.Z().to(), 0.01); - ASSERT_NEAR(wpi::units::degree_t{5}.convert().to(), - pose.Rotation().Z().to(), 0.01); + ASSERT_NEAR( + wpi::units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); @@ -510,8 +523,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); ASSERT_NEAR(0, pose2.Z().to(), 0.01); - ASSERT_NEAR(wpi::units::degree_t{5}.convert().to(), - pose2.Rotation().Z().to(), 0.01); + ASSERT_NEAR( + wpi::units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); } TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { @@ -528,20 +542,20 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { std::vector tagList; tagList.emplace_back(wpi::apriltag::AprilTag{ - 0, - wpi::math::Pose3d{12_m, 3_m, 1_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 0, wpi::math::Pose3d{ + 12_m, 3_m, 1_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); tagList.emplace_back(wpi::apriltag::AprilTag{ - 1, - wpi::math::Pose3d{12_m, 1_m, -1_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 1, wpi::math::Pose3d{ + 12_m, 1_m, -1_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); tagList.emplace_back(wpi::apriltag::AprilTag{ - 2, - wpi::math::Pose3d{11_m, 0_m, 2_m, - wpi::math::Rotation3d{ - 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + 2, wpi::math::Pose3d{ + 11_m, 0_m, 2_m, + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}}); wpi::units::meter_t fieldLength{54}; wpi::units::meter_t fieldWidth{27}; wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; @@ -572,8 +586,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { ASSERT_NEAR(5, pose.X().to(), 0.01); ASSERT_NEAR(1, pose.Y().to(), 0.01); ASSERT_NEAR(0, pose.Z().to(), 0.01); - ASSERT_NEAR(wpi::units::degree_t{-5}.convert().to(), - pose.Rotation().Z().to(), 0.01); + ASSERT_NEAR( + wpi::units::degree_t{-5}.convert().to(), + pose.Rotation().Z().to(), 0.01); visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag36h11, 1}}); @@ -595,8 +610,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); ASSERT_NEAR(0, pose2.Z().to(), 0.01); - ASSERT_NEAR(wpi::units::degree_t{-5}.convert().to(), - pose2.Rotation().Z().to(), 0.01); + ASSERT_NEAR( + wpi::units::degree_t{-5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); } TEST_F(VisionSystemSimTest, TestTagAmbiguity) { @@ -609,7 +625,8 @@ TEST_F(VisionSystemSimTest, TestTagAmbiguity) { wpi::math::Pose3d targetPose{ wpi::math::Translation3d{2_m, 0_m, 0_m}, - wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Rotation3d{0_rad, 0_rad, + wpi::units::radian_t{std::numbers::pi}}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}}); diff --git a/photon-server/build.gradle b/photon-server/build.gradle index 77a137a28f..d38d34b44b 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -1,6 +1,11 @@ +import org.wpilib.deployutils.deploy.artifact.* +import org.wpilib.deployutils.deploy.target.RemoteTarget +import org.wpilib.deployutils.deploy.target.location.SshDeployLocation + apply plugin: "com.github.node-gradle.node" apply plugin: 'com.gradleup.shadow' apply plugin: "application" +apply plugin: 'org.wpilib.DeployUtils' apply from: "${rootDir}/shared/common.gradle" @@ -75,9 +80,6 @@ run { } } -import org.wpilib.deployutils.deploy.artifact.* -import org.wpilib.deployutils.deploy.target.RemoteTarget -import org.wpilib.deployutils.deploy.target.location.SshDeployLocation deploy { targets { pi(RemoteTarget) { diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 9c353515eb..d3eacb838b 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -55,6 +55,7 @@ import org.photonvision.vision.processes.VisionSourceManager; import org.photonvision.vision.target.TargetModel; import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Rotation2d; public class Main { public static final int DEFAULT_WEBPORT = 5800; @@ -242,8 +243,6 @@ public static void main(String[] args) { if (OsImageData.IMAGE_METADATA.isPresent()) { logger.info("PhotonVision image data: " + OsImageData.IMAGE_METADATA.get()); - } else if (OsImageData.IMAGE_VERSION.isPresent()) { - logger.info("PhotonVision image version: " + OsImageData.IMAGE_VERSION.get()); } else { logger.info("PhotonVision image version: unknown"); } diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 6fcab1a46a..33cca23209 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -169,7 +169,7 @@ cppHeadersZip { } // setup wpilib bundled native libs -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() +wpilibTools.deps.wpilibVersion = wpilibVersion def nativeConfigName = 'wpilibNatives' def nativeConfig = configurations.create(nativeConfigName) @@ -187,5 +187,5 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") -nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get()) +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion) nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java index 38269c428b..bf1c0696fe 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java @@ -28,13 +28,12 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; /** diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java index 8d26d29818..fb8e811dd9 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -28,13 +28,12 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; /** diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 17c3aba09f..ec0371fe2f 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -28,13 +28,12 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; /** diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java index ed08a8b58c..5d1c3a84bf 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java @@ -34,7 +34,7 @@ import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; import org.wpilib.math.geometry.Transform3d; /** diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java index 73a2d65cb0..90f4b7a6f0 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java @@ -34,7 +34,7 @@ import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; import org.wpilib.math.geometry.Transform3d; /** diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java index c276b1e211..df42d7f46f 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java @@ -28,13 +28,12 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; /** diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h index 468be90211..6a03b44d70 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h @@ -36,7 +36,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h index 8dee1e8a49..554a266ab9 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h @@ -34,7 +34,7 @@ // Includes for dependant types #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h index 40586156d8..a9eace8962 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h @@ -30,7 +30,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h index 7da4c5ad0a..b6442585d3 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h @@ -28,7 +28,7 @@ // Includes for dependant types #include -#include +#include namespace photon { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index ff450adf2d..4e7354ce9d 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -85,15 +85,15 @@ public void updateEntries() { .publish( PhotonPipelineResult.photonStruct.getTypeString(), PubSubOption.periodic(0.01), - PubSubOption.sendAll(true), - PubSubOption.keepDuplicates(true)); + PubSubOption.SEND_ALL, + PubSubOption.KEEP_DUPLICATES); resultPublisher = new PacketPublisher(rawBytesEntry, PhotonPipelineResult.photonStruct); protoResultPublisher = subTable .getProtobufTopic("result_proto", PhotonPipelineResult.proto) - .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .publish(PubSubOption.periodic(0.01), PubSubOption.SEND_ALL); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 857f48c8a5..0c9ef4261c 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -48,7 +48,6 @@ import org.wpilib.math.numbers.*; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Num; -import org.wpilib.vision.camera.OpenCvLoader; /** * A set of various utility functions for getting non-OpenCV data into an OpenCV-compatible format, @@ -269,7 +268,7 @@ public static List reorderCircular(List elements, boolean backwards, i * @return The converted rotation in the NWU coordinate system */ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { - return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU)); + return EDN_TO_NWU.inverse().rotateBy(rot.rotateBy(EDN_TO_NWU)); } /** @@ -279,7 +278,7 @@ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { * @return The converted rotation in the EDN coordinate system */ private static Rotation3d rotationNWUtoEDN(Rotation3d rot) { - return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN)); + return NWU_TO_EDN.inverse().rotateBy(rot.rotateBy(NWU_TO_EDN)); } /** diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index 884b35276b..72c5469b5d 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -46,7 +46,7 @@ public RotTrlTransform3d(Rotation3d rot, Translation3d trl) { } public RotTrlTransform3d(Pose3d initial, Pose3d last) { - this.rot = last.getRotation().minus(initial.getRotation()); + this.rot = last.getRotation().relativeTo(initial.getRotation()); this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot)); } @@ -83,7 +83,7 @@ public static RotTrlTransform3d makeRelativeTo(Pose3d pose) { * @return The inverse transformation */ public RotTrlTransform3d inverse() { - var inverseRot = rot.unaryMinus(); + var inverseRot = rot.inverse(); var inverseTrl = trl.rotateBy(inverseRot).unaryMinus(); return new RotTrlTransform3d(inverseRot, inverseTrl); } @@ -124,7 +124,7 @@ public List applyTrls(List trls) { } public Rotation3d apply(Rotation3d rot) { - return rot.plus(this.rot); + return rot.rotateBy(this.rot); } public List applyRots(List rots) { diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp index 9df7724813..e82c9ab27a 100644 --- a/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp +++ b/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp @@ -106,7 +106,8 @@ void wpi::tsp::TimeSyncClient::Tick() { m_lastPing = ping; } -void wpi::tsp::TimeSyncClient::UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, +void wpi::tsp::TimeSyncClient::UdpCallback(wpi::net::uv::Buffer& buf, + size_t nbytes, const sockaddr& sender, unsigned flags) { uint64_t pong_local_time = m_timeProvider(); @@ -183,8 +184,9 @@ void wpi::tsp::TimeSyncClient::Start() { using namespace std::chrono_literals; m_pingTimer->timeout.connect(&wpi::tsp::TimeSyncClient::Tick, this); - m_loopRunner.ExecSync( - [this](wpi::net::uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); }); + m_loopRunner.ExecSync([this](wpi::net::uv::Loop&) { + m_pingTimer->Start(m_loopDelay, m_loopDelay); + }); } void wpi::tsp::TimeSyncClient::Stop() { m_loopRunner.Stop(); } diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp index 70e4f7974e..31c8e5469d 100644 --- a/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp +++ b/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp @@ -23,11 +23,12 @@ #include #include +#include #include #include #include -#include +#include "net/TimeSyncStructs.h" static void ServerLoggerFunc(unsigned int level, const char* file, unsigned int line, const char* msg) { diff --git a/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp b/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp index aefa359be4..e92d524427 100644 --- a/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp +++ b/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include "../generate/constrained_solvepnp_10_tags_fixed.h" #include "../generate/constrained_solvepnp_10_tags_free.h" @@ -47,6 +47,7 @@ #include "../generate/constrained_solvepnp_8_tags_free.h" #include "../generate/constrained_solvepnp_9_tags_fixed.h" #include "../generate/constrained_solvepnp_9_tags_free.h" +#include "wpi/nt/ntcore_cpp.hpp" constexpr bool VERBOSE = false; @@ -225,7 +226,7 @@ constrained_solvepnp::do_optimization( constexpr double ERROR_TOL = 1e-4; for (int iter = 0; iter < 100; iter++) { - auto iter_start = wpi::util::Now(); + auto iter_start = wpi::nt::Now(); // Check for diverging iterates if (x.template lpNorm() > 1e20 || !x.allFinite()) { @@ -343,7 +344,7 @@ constrained_solvepnp::do_optimization( } } - auto iter_end = wpi::util::Now(); + auto iter_end = wpi::nt::Now(); if constexpr (VERBOSE) { fmt::println( "{}: {} uS, ‖∇J‖={}, α={} ({} refinement steps), {} regularization " diff --git a/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp index b65f2f388b..975610bafc 100644 --- a/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp +++ b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp @@ -191,8 +191,8 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( guess2.X().value(), guess2.Y().value(), guess2.Rotation().Radians().value()}; - wpi::util::expected result = - constrained_solvepnp::do_optimization( + wpi::util::expected + result = constrained_solvepnp::do_optimization( headingFree, knownTags.size(), cameraCal, robotToCamera, guessMat, field2points, pointObservations, gyroTheta.Radians().value(), gyroErrorScaleFac); @@ -203,7 +203,8 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( photon::PnpResult res{}; res.best = wpi::math::Transform3d{wpi::math::Transform2d{ - wpi::units::meter_t{result.value()[0]}, wpi::units::meter_t{result.value()[1]}, + wpi::units::meter_t{result.value()[0]}, + wpi::units::meter_t{result.value()[1]}, wpi::math::Rotation2d{wpi::units::radian_t{result.value()[2]}}}}; return res; diff --git a/photon-targeting/src/main/native/include/net/TimeSyncServer.h b/photon-targeting/src/main/native/include/net/TimeSyncServer.h index 2bb8ccc35f..b048b0b481 100644 --- a/photon-targeting/src/main/native/include/net/TimeSyncServer.h +++ b/photon-targeting/src/main/native/include/net/TimeSyncServer.h @@ -45,8 +45,8 @@ class TimeSyncServer { std::thread m_listener; private: - void UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, const sockaddr& sender, - unsigned flags); + void UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, + const sockaddr& sender, unsigned flags); public: explicit TimeSyncServer(int port = 5810); diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h index d073bba99b..8bd0123159 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -43,7 +43,7 @@ class CameraTargetRelation { camToTarg(wpi::math::Transform3d{cameraPose, targetPose}), camToTargDist(camToTarg.Translation().Norm()), camToTargDistXY(wpi::units::math::hypot(camToTarg.Translation().X(), - camToTarg.Translation().Y())), + camToTarg.Translation().Y())), camToTargYaw(wpi::math::Rotation2d{camToTarg.X().to(), camToTarg.Y().to()}), camToTargPitch(wpi::math::Rotation2d{camToTargDistXY.to(), diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index f63b7482a4..059376dde2 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -64,7 +64,7 @@ static wpi::math::Translation3d TranslationNWUtoEDN( static wpi::math::Rotation3d RotationNWUtoEDN( const wpi::math::Rotation3d& rot) { - return -NWU_TO_EDN + (rot + NWU_TO_EDN); + return NWU_TO_EDN.Inverse().RotateBy(rot.RotateBy(NWU_TO_EDN)); } static std::vector TranslationToTVec( @@ -186,7 +186,7 @@ static wpi::math::Translation3d TranslationEDNToNWU( static wpi::math::Rotation3d RotationEDNToNWU( const wpi::math::Rotation3d& rot) { - return -EDN_TO_NWU + (rot + EDN_TO_NWU); + return EDN_TO_NWU.Inverse().RotateBy(rot.RotateBy(EDN_TO_NWU)); } static wpi::math::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { @@ -194,9 +194,9 @@ static wpi::math::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; tvecInput.convertTo(wrapped, CV_32F); data = wrapped.at(cv::Point{0, 0}); - return TranslationEDNToNWU(wpi::math::Translation3d{wpi::units::meter_t{data[0]}, - wpi::units::meter_t{data[1]}, - wpi::units::meter_t{data[2]}}); + return TranslationEDNToNWU(wpi::math::Translation3d{ + wpi::units::meter_t{data[0]}, wpi::units::meter_t{data[1]}, + wpi::units::meter_t{data[2]}}); } static wpi::math::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h index 0f5d733fc6..b735487396 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -32,9 +32,10 @@ class RotTrlTransform3d { RotTrlTransform3d(const wpi::math::Pose3d& initial, const wpi::math::Pose3d& last) - : trl{last.Translation() - initial.Translation().RotateBy( - last.Rotation() - initial.Rotation())}, - rot{last.Rotation() - initial.Rotation()} {} + : trl{last.Translation() - + initial.Translation().RotateBy( + last.Rotation().RelativeTo(initial.Rotation()))}, + rot{last.Rotation().RelativeTo(initial.Rotation())} {} explicit RotTrlTransform3d(const wpi::math::Transform3d& trf) : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} RotTrlTransform3d() @@ -46,7 +47,7 @@ class RotTrlTransform3d { } RotTrlTransform3d Inverse() const { - wpi::math::Rotation3d invRot = -rot; + wpi::math::Rotation3d invRot = rot.Inverse(); wpi::math::Translation3d invTrl = -(trl.RotateBy(invRot)); return RotTrlTransform3d{invRot, invTrl}; } @@ -75,7 +76,7 @@ class RotTrlTransform3d { } wpi::math::Rotation3d Apply(const wpi::math::Rotation3d& rotToApply) const { - return rotToApply + rot; + return rotToApply.RotateBy(rot); } std::vector ApplyTrls( diff --git a/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp b/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp index 66041553fb..a4c4b8ea6b 100644 --- a/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp +++ b/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp @@ -41,11 +41,12 @@ extern "C" { * Signature: (ZI[D[D[D[D[DDD)[D */ JNIEXPORT jdoubleArray JNICALL -Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization( - JNIEnv* env, jclass, jboolean headingFree, jint nTags, - jdoubleArray cameraCal, jdoubleArray robot2camera, jdoubleArray xGuess, - jdoubleArray field2points, jdoubleArray pointObservations, jdouble gyro_θ, - jdouble gyro_error_scale_fac) { +Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization + (JNIEnv* env, jclass, jboolean headingFree, jint nTags, + jdoubleArray cameraCal, jdoubleArray robot2camera, jdoubleArray xGuess, + jdoubleArray field2points, jdoubleArray pointObservations, jdouble gyro_θ, + jdouble gyro_error_scale_fac) +{ auto cameraCalVec = convertJDoubleArray(env, cameraCal); auto robot2cameraVec = convertJDoubleArray(env, robot2camera); auto xGuessVec = convertJDoubleArray(env, xGuess); diff --git a/photon-targeting/src/main/native/jni/CscoreExtras.cpp b/photon-targeting/src/main/native/jni/CscoreExtras.cpp index 748e06c315..5bbf40169c 100644 --- a/photon-targeting/src/main/native/jni/CscoreExtras.cpp +++ b/photon-targeting/src/main/native/jni/CscoreExtras.cpp @@ -18,10 +18,11 @@ #include #include +#include #include #include "org_photonvision_jni_CscoreExtras.h" -#include "wpi/cs/cscore_raw.h" +#include "wpi/cs/cscore_raw.hpp" // from wpilib, licensed under the wpilib BSD license using namespace wpi::util::java; diff --git a/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp b/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp index 9a22daa4b4..a8fe9d8d1d 100644 --- a/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp +++ b/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp @@ -15,8 +15,8 @@ * along with this program. If not, see . */ -#include #include +#include #include #include @@ -159,15 +159,15 @@ void print_cost(casadi_real robot_x, casadi_real robot_y, x_guess << robot_x, robot_y, robot_theta; for (int i = 0; i < 200; i++) { - auto start = wpi::util::Now(); + auto start = WPI_Now(); auto x_out = constrained_solvepnp::do_optimization( true, TAG_COUNT, constrained_solvepnp::CameraCalibration{fx, fy, cx, cy}, robot2camera, x_guess, field2points, point_observations, 0, 0); - auto end = wpi::util::Now(); + auto end = WPI_Now(); - wpi::println("{},{},{}", i, static_cast(x_out), end - start); - wpi::println( + wpi::util::println("{},{},{}", i, static_cast(x_out), end - start); + wpi::util::println( "Solution: {}", x_out.value_or(constrained_solvepnp::RobotStateMat::Identity())); // std::cout << "iter " diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index b6222c17d7..1eea95c6b3 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-cpp-examples/aimandrange/settings.gradle b/photonlib-cpp-examples/aimandrange/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-cpp-examples/aimandrange/settings.gradle +++ b/photonlib-cpp-examples/aimandrange/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp index 387b9897cf..07646b088b 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp @@ -124,7 +124,8 @@ void Robot::SimulationPeriodic() { wpi::sim::BatterySim::Calculate({totalCurrent}); // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage( + wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp index 4703597d09..e2f21871b8 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp @@ -151,7 +151,8 @@ void SwerveDrive::Log() { wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); + chassisSpeeds.omega.convert() + .to()); wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); wpi::SmartDashboard::PutNumber(table + "Target VY", @@ -207,4 +208,6 @@ wpi::math::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } -wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { + return totalCurrentDraw; +} diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp index cdcb81acd5..6fdb2be372 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -60,11 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), @@ -230,7 +231,9 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { + return omega; +} wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, @@ -245,8 +248,8 @@ wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() - const { +std::array +SwerveDriveSim::GetDriveCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = @@ -258,8 +261,8 @@ std::array SwerveDriveSim::GetDriveCurrentDraw return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() - const { +std::array +SwerveDriveSim::GetSteerCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp index 3bfa786b22..a824d20a75 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp @@ -71,7 +71,8 @@ void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, } wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; + return wpi::math::Rotation2d{ + wpi::units::radian_t{steerEncoder.GetDistance()}}; } wpi::math::SwerveModuleState SwerveModule::GetState() const { @@ -132,8 +133,8 @@ void SwerveModule::Log() { void SwerveModule::SimulationUpdate( wpi::units::meter_t driveEncoderDist, - wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, - wpi::units::radian_t steerEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, wpi::units::radian_t steerEncoderDist, wpi::units::radians_per_second_t steerEncoderRate, wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h index 1e27703dfa..d55d4fa7ea 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h @@ -58,10 +58,11 @@ inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{ + 720_deg_per_s}; inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * - std::numbers::pi}; + std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; @@ -101,8 +102,8 @@ struct ModuleConstants { ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, - wpi::units::meter_t yOffset) + int steerEncoderB, double angleOffset, + wpi::units::meter_t xOffset, wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h index 6a68e88150..0718640667 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -35,7 +35,8 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + void Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, wpi::units::radians_per_second_t omega); void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h index 5ace0164dd..61339147b8 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h @@ -44,11 +44,12 @@ class SwerveDriveSim { const wpi::math::DCMotor& steerMotor, double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); void SetDriveInputs(const std::array& inputs); void SetSteerInputs(const std::array& inputs); @@ -75,9 +76,9 @@ class SwerveDriveSim { std::array, numModules> GetSteerStates() const; wpi::units::radians_per_second_t GetOmega() const; wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, - wpi::units::radians_per_second_t velocity, - wpi::units::volt_t inputVolts, - wpi::units::volt_t batteryVolts) const; + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; std::array GetDriveCurrentDraw() const; std::array GetSteerCurrentDraw() const; wpi::units::ampere_t GetTotalCurrentDraw() const; diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index b6222c17d7..1eea95c6b3 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-cpp-examples/aimattarget/settings.gradle b/photonlib-cpp-examples/aimattarget/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-cpp-examples/aimattarget/settings.gradle +++ b/photonlib-cpp-examples/aimattarget/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index acb89dc9bc..617e0737b0 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -115,7 +115,8 @@ void Robot::SimulationPeriodic() { wpi::sim::BatterySim::Calculate({totalCurrent}); // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage( + wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp index 2d594a992e..15c7b4888d 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp @@ -151,7 +151,8 @@ void SwerveDrive::Log() { wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); + chassisSpeeds.omega.convert() + .to()); wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); wpi::SmartDashboard::PutNumber(table + "Target VY", @@ -207,4 +208,6 @@ wpi::math::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } -wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { + return totalCurrentDraw; +} diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp index 4c98f0ce20..18d2c68add 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -60,11 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), @@ -230,7 +231,9 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { + return omega; +} wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, @@ -245,8 +248,8 @@ wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() - const { +std::array +SwerveDriveSim::GetDriveCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = @@ -258,8 +261,8 @@ std::array SwerveDriveSim::GetDriveCurrentDraw return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() - const { +std::array +SwerveDriveSim::GetSteerCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp index 3bfa786b22..a824d20a75 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp @@ -71,7 +71,8 @@ void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, } wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; + return wpi::math::Rotation2d{ + wpi::units::radian_t{steerEncoder.GetDistance()}}; } wpi::math::SwerveModuleState SwerveModule::GetState() const { @@ -132,8 +133,8 @@ void SwerveModule::Log() { void SwerveModule::SimulationUpdate( wpi::units::meter_t driveEncoderDist, - wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, - wpi::units::radian_t steerEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, wpi::units::radian_t steerEncoderDist, wpi::units::radians_per_second_t steerEncoderRate, wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h index 1e27703dfa..d55d4fa7ea 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h @@ -58,10 +58,11 @@ inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{ + 720_deg_per_s}; inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * - std::numbers::pi}; + std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; @@ -101,8 +102,8 @@ struct ModuleConstants { ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, - wpi::units::meter_t yOffset) + int steerEncoderB, double angleOffset, + wpi::units::meter_t xOffset, wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h index 09ebbb0402..8422889ae8 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h @@ -37,7 +37,8 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + void Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, wpi::units::radians_per_second_t omega); void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h index 5ace0164dd..61339147b8 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h @@ -44,11 +44,12 @@ class SwerveDriveSim { const wpi::math::DCMotor& steerMotor, double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); void SetDriveInputs(const std::array& inputs); void SetSteerInputs(const std::array& inputs); @@ -75,9 +76,9 @@ class SwerveDriveSim { std::array, numModules> GetSteerStates() const; wpi::units::radians_per_second_t GetOmega() const; wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, - wpi::units::radians_per_second_t velocity, - wpi::units::volt_t inputVolts, - wpi::units::volt_t batteryVolts) const; + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; std::array GetDriveCurrentDraw() const; std::array GetSteerCurrentDraw() const; wpi::units::ampere_t GetTotalCurrentDraw() const; diff --git a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties index 94113f200e..bad7c2462f 100644 --- a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photonlib-cpp-examples/poseest/build.gradle b/photonlib-cpp-examples/poseest/build.gradle index b6222c17d7..1eea95c6b3 100644 --- a/photonlib-cpp-examples/poseest/build.gradle +++ b/photonlib-cpp-examples/poseest/build.gradle @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-cpp-examples/poseest/settings.gradle b/photonlib-cpp-examples/poseest/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-cpp-examples/poseest/settings.gradle +++ b/photonlib-cpp-examples/poseest/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 4bf65dc3b1..43d0bc7b5c 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp @@ -98,7 +98,8 @@ void Robot::SimulationPeriodic() { // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage( + wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp index c2a04955b7..f944d5fdc6 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp @@ -35,10 +35,10 @@ void GamepieceLauncher::setRunning(bool shouldRun) { void GamepieceLauncher::periodic() { // Calculate the maximum RPM - double maxRPM = - wpi::units::radians_per_second_t(wpi::math::DCMotor::Falcon500(1).freeSpeed) - .to() * - 60 / (2 * std::numbers::pi); + double maxRPM = wpi::units::radians_per_second_t( + wpi::math::DCMotor::Falcon500(1).freeSpeed) + .to() * + 60 / (2 * std::numbers::pi); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0); motor->Set(curMotorCmd); diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp index 19fe852160..45d92940f5 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp @@ -81,7 +81,8 @@ void SwerveDrive::SetModuleStates( void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } void SwerveDrive::AddVisionMeasurement( - const wpi::math::Pose2d& visionMeasurement, wpi::units::second_t timestamp) { + const wpi::math::Pose2d& visionMeasurement, + wpi::units::second_t timestamp) { poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); } @@ -163,7 +164,8 @@ void SwerveDrive::Log() { wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); + chassisSpeeds.omega.convert() + .to()); wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); wpi::SmartDashboard::PutNumber(table + "Target VY", @@ -219,4 +221,6 @@ wpi::math::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } -wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { + return totalCurrentDraw; +} diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp index 9f3713c888..d0ed289509 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -60,11 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), @@ -230,7 +231,9 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { + return omega; +} wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, @@ -245,8 +248,8 @@ wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() - const { +std::array +SwerveDriveSim::GetDriveCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = @@ -258,8 +261,8 @@ std::array SwerveDriveSim::GetDriveCurrentDraw return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() - const { +std::array +SwerveDriveSim::GetSteerCurrentDraw() const { std::array currents; for (int i = 0; i < numModules; i++) { wpi::units::radians_per_second_t speed = diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp index 3bfa786b22..a824d20a75 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp @@ -71,7 +71,8 @@ void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, } wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; + return wpi::math::Rotation2d{ + wpi::units::radian_t{steerEncoder.GetDistance()}}; } wpi::math::SwerveModuleState SwerveModule::GetState() const { @@ -132,8 +133,8 @@ void SwerveModule::Log() { void SwerveModule::SimulationUpdate( wpi::units::meter_t driveEncoderDist, - wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, - wpi::units::radian_t steerEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, wpi::units::radian_t steerEncoderDist, wpi::units::radians_per_second_t steerEncoderRate, wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); diff --git a/photonlib-cpp-examples/poseest/src/main/include/Constants.h b/photonlib-cpp-examples/poseest/src/main/include/Constants.h index 1e27703dfa..d55d4fa7ea 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Constants.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Constants.h @@ -58,10 +58,11 @@ inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{ + 720_deg_per_s}; inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * - std::numbers::pi}; + std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; @@ -101,8 +102,8 @@ struct ModuleConstants { ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, - wpi::units::meter_t yOffset) + int steerEncoderB, double angleOffset, + wpi::units::meter_t xOffset, wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), diff --git a/photonlib-cpp-examples/poseest/src/main/include/Robot.h b/photonlib-cpp-examples/poseest/src/main/include/Robot.h index 284f920325..b7da573fbc 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Robot.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Robot.h @@ -51,7 +51,8 @@ class Robot : public wpi::TimedRobot { private: SwerveDrive drivetrain{}; - Vision vision{[=, this](wpi::math::Pose2d pose, wpi::units::second_t timestamp, + Vision vision{[=, this](wpi::math::Pose2d pose, + wpi::units::second_t timestamp, Eigen::Matrix stddevs) { drivetrain.AddVisionMeasurement(pose, timestamp, stddevs); }}; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h index 259449dfe4..4f92bfa0f3 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h @@ -50,8 +50,8 @@ class GamepieceLauncher { double curDesSpd = 0.0; double curMotorCmd = 0.0; - static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia = - 0.5 * 1.5_lb * 4_in * 4_in; + static constexpr wpi::units::kilogram_square_meter_t + kFlywheelMomentOfInertia = 0.5 * 1.5_lb * 4_in * 4_in; wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::Falcon500(1); wpi::math::LinearSystem<1, 1, 1> m_plant{ diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h index 9d39b8dda0..cc65085bb9 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h @@ -37,7 +37,8 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + void Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, wpi::units::radians_per_second_t omega); void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h index 5ace0164dd..61339147b8 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h @@ -44,11 +44,12 @@ class SwerveDriveSim { const wpi::math::DCMotor& steerMotor, double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); SwerveDriveSim( - const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, - const wpi::math::DCMotor& driveMotor, double driveGearing, - wpi::units::meter_t driveWheelRadius, - const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, - const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, + wpi::units::volt_t driveKs, const wpi::math::DCMotor& driveMotor, + double driveGearing, wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, + wpi::units::volt_t steerKs, const wpi::math::DCMotor& steerMotor, + double steerGearing, const wpi::math::SwerveDriveKinematics& kinematics); void SetDriveInputs(const std::array& inputs); void SetSteerInputs(const std::array& inputs); @@ -75,9 +76,9 @@ class SwerveDriveSim { std::array, numModules> GetSteerStates() const; wpi::units::radians_per_second_t GetOmega() const; wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, - wpi::units::radians_per_second_t velocity, - wpi::units::volt_t inputVolts, - wpi::units::volt_t batteryVolts) const; + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; std::array GetDriveCurrentDraw() const; std::array GetSteerCurrentDraw() const; wpi::units::ampere_t GetTotalCurrentDraw() const; diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index b6983ba2d8..9839bece48 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -4,8 +4,8 @@ plugins { } java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_21 + targetCompatibility = JavaVersion.VERSION_21 } def ROBOT_MAIN_CLASS = "frc.robot.Main" @@ -15,7 +15,7 @@ repositories { } wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties index 34bd9ce95f..0c795facac 100644 --- a/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photonlib-java-examples/aimandrange/settings.gradle b/photonlib-java-examples/aimandrange/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-java-examples/aimandrange/settings.gradle +++ b/photonlib-java-examples/aimandrange/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' 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 5780d66e7c..3691195a2e 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -4,14 +4,14 @@ plugins { } java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_21 + targetCompatibility = JavaVersion.VERSION_21 } def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties index 34bd9ce95f..0c795facac 100644 --- a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photonlib-java-examples/aimattarget/settings.gradle b/photonlib-java-examples/aimattarget/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-java-examples/aimattarget/settings.gradle +++ b/photonlib-java-examples/aimattarget/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' 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 94113f200e..bad7c2462f 100644 --- a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index 5780d66e7c..3691195a2e 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -4,14 +4,14 @@ plugins { } java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_21 + targetCompatibility = JavaVersion.VERSION_21 } def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.+" +wpi.versions.wpilibVersion = "2027.0.0-alpha-4" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties index 34bd9ce95f..0c795facac 100644 --- a/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.0-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/photonlib-java-examples/poseest/settings.gradle b/photonlib-java-examples/poseest/settings.gradle index 64213ce87a..2374b33a8f 100644 --- a/photonlib-java-examples/poseest/settings.gradle +++ b/photonlib-java-examples/poseest/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2027_alpha1' + String frcYear = '2027_alpha4' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/shared/common.gradle b/shared/common.gradle index 125d708d48..87a5613ce2 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -3,16 +3,15 @@ apply plugin: "java" apply plugin: "jacoco" java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + toolchain { + languageVersion = JavaLanguageVersion.of(21) + } + sourceCompatibility = JavaVersion.VERSION_21 + targetCompatibility = JavaVersion.VERSION_21 } wpilibTools.deps.wpilibVersion = wpilibVersion -// Tell gradlerio what version of things to use (that we care about) -// See: https://github.com/wpilibsuite/GradleRIO/blob/main/src/main/java/edu/wpi/first/gradlerio/wpi/WPIVersionsExtension.java -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); dependencies { implementation project(':photon-targeting') @@ -33,14 +32,14 @@ dependencies { implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion" implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion" implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion" - implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, wpi.versions.opencvVersion.get()) + implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, openCVversion) - implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: jacksonVersion + implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion + implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); + implementation group: "org.ejml", name: "ejml-simple", version: ejmlVersion + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: quickbufVersion; implementation "commons-io:commons-io:2.11.0" implementation "commons-cli:commons-cli:1.5.0" diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 480a60d3c9..dcb15d1bf8 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -4,8 +4,11 @@ apply plugin: 'jacoco' apply plugin: 'com.google.protobuf' java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + toolchain { + languageVersion = JavaLanguageVersion.of(21) + } + sourceCompatibility = JavaVersion.VERSION_21 + targetCompatibility = JavaVersion.VERSION_21 } def baseArtifactId = nativeName @@ -126,7 +129,7 @@ test { finalizedBy jacocoTestReport } -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() +wpilibTools.deps.wpilibVersion = wpilibVersion dependencies { if(project.hasProperty('includePhotonTargeting')) { @@ -144,19 +147,20 @@ dependencies { implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion" implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion" implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion" - implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, wpi.versions.opencvVersion.get()) + implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, openCVversion) - implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: jacksonVersion + implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion + implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); + implementation group: "org.ejml", name: "ejml-simple", version: ejmlVersion + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: quickbufVersion; testImplementation(platform('org.junit:junit-bom:5.11.4')) testImplementation 'org.junit.jupiter:junit-jupiter-api' testImplementation 'org.junit.jupiter:junit-jupiter-params' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } jacoco { diff --git a/versioningHelper.gradle b/versioningHelper.gradle index b2e5e01994..4a91834c17 100644 --- a/versioningHelper.gradle +++ b/versioningHelper.gradle @@ -2,45 +2,41 @@ import java.nio.file.Path import java.time.LocalDateTime import java.time.format.DateTimeFormatter -gradle.allprojects { - ext.getCurrentVersion = { - -> - def stdout = new ByteArrayOutputStream() - String tagIsh - try { - project.exec { - commandLine 'git', 'describe', '--tags', "--match=v*" - standardOutput = stdout - } - tagIsh = stdout.toString().trim().toLowerCase() - } catch (Exception e) { - tagIsh = "dev-Unknown" - } - - // Dev tags: v2021.1.6-3-gf922466d - // We're specifically looking to capture the middle -3- - boolean isDev = tagIsh.matches(".*-[0-9a-z]*-g[0-9a-f]*") - if (isDev && !tagIsh.startsWith("dev-")) tagIsh = "dev-" + tagIsh - println("Picked up version: " + tagIsh) - return tagIsh +ext.getCurrentVersion = { + String tagIsh = "dev-unknown" + try { + tagIsh = providers.exec { + commandLine 'git', 'describe', '--tags', '--match=v*' + }.standardOutput.asText.get().trim().toLowerCase() + } catch (Exception ignored) { + tagIsh = "dev-unknown" } - if (!ext.has("versionString")) { - ext.versionString = getCurrentVersion() - } + // Dev tags: v2021.1.6-3-gf922466d + // We're specifically looking to capture the middle -3- + boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*") + if (isDev && !tagIsh.startsWith("dev-")) tagIsh = "dev-" + tagIsh + println("Picked up version: " + tagIsh) + return tagIsh +} - ext.writePhotonVersionFile = {File versionFileIn, Path path, String version -> - println("Writing " + version + " to " + path.toAbsolutePath().toString()) - String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now()) - File versionFileOut = new File(path.toAbsolutePath().toString()) - versionFileOut.delete() - def read = versionFileIn.text.replace('${version}', version) - .replace('${date}', date) - .replace('${wpilibVersion}', wpilibVersion) - // Note that OpenCV is usually {VERSION}-{some suffix}, we just want the first bit - .replace('${opencvVersion}', openCVversion.split("-").first()) - if (!versionFileOut.parentFile.exists()) versionFileOut.parentFile.mkdirs() - if (!versionFileOut.exists()) versionFileOut.createNewFile() - versionFileOut.write(read) - } +if (!ext.has("versionString")) { + ext.versionString = getCurrentVersion() +} + +ext.writePhotonVersionFile = {File versionFileIn, Path path, String version -> + println("Writing " + version + " to " + path.toAbsolutePath().toString()) + String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now()) + File versionFileOut = new File(path.toAbsolutePath().toString()) + versionFileOut.delete() + + def read = versionFileIn.text + .replace('${version}', version) + .replace('${date}', date) + .replace('${wpilibVersion}', wpilibVersion) + // Note that OpenCV is usually {VERSION}-{some suffix}, we just want the first bit + .replace('${opencvVersion}', openCVversion.split("-").first()) + if (!versionFileOut.parentFile.exists()) versionFileOut.parentFile.mkdirs() + if (!versionFileOut.exists()) versionFileOut.createNewFile() + versionFileOut.write(read) }