From e9006a2803c586081ec367c23ed1da9617caefd5 Mon Sep 17 00:00:00 2001 From: Sam Freund Date: Tue, 26 May 2026 21:47:48 -0400 Subject: [PATCH] Upgrade to wpilib alpha-6 (#2434) Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Ryanforce08 Co-authored-by: PJ Reiniger Co-authored-by: Jade Turner Co-authored-by: Matt Morley --- .github/workflows/build.yml | 152 +++++----- .github/workflows/python.yml | 264 +++++++++--------- README.md | 6 +- build.gradle | 24 +- .../sw_install/linux-pc.md | 2 +- .../sw_install/windows-pc.md | 2 +- .../design-descriptions/e2e-latency.md | 4 +- photon-core/build.gradle | 12 +- .../java/org/photonvision/common/LoadJNI.java | 2 +- .../networktables/NTDataChangeListener.java | 2 +- .../networktables/NTDriverStation.java | 4 +- .../networktables/NetworkTablesManager.java | 26 +- .../networktables/TimeSyncManager.java | 2 +- .../common/networking/NetworkUtils.java | 4 +- .../file/AprilTagFieldLayoutJsonAdapter.java | 83 ------ .../common/util/file/Pose3dJsonAdapter.java | 103 ------- .../common/util/file/QuaternionMixIn.java | 45 --- .../common/util/file/Rotation3dMixIn.java | 34 --- .../common/util/file/Translation3dMixIn.java | 41 --- .../vision/camera/FileVisionSource.java | 2 +- .../USBCameras/GenericUSBCameraSettables.java | 4 +- .../camera/csi/LibcameraGpuSettables.java | 30 +- .../frame/consumer/FileSaveFrameConsumer.java | 4 +- .../frame/consumer/MJPGFrameConsumer.java | 2 +- .../frame/provider/USBFrameProvider.java | 2 +- .../vision/objects/RubikObjectDetector.java | 15 +- .../vision/pipe/impl/CalculateFPSPipe.java | 4 +- .../vision/processes/VisionModule.java | 5 +- .../consumer/FileSaveFrameConsumerTest.java | 6 +- photon-lib/build.gradle | 6 +- .../py/photonlibpy/estimatedRobotPose.py | 2 +- .../estimation/cameraTargetRelation.py | 2 +- .../py/photonlibpy/estimation/openCVHelp.py | 6 +- .../estimation/rotTrlTransform3d.py | 12 +- .../py/photonlibpy/estimation/targetModel.py | 2 +- .../estimation/visionEstimation.py | 2 +- .../generated/PhotonPipelineResultSerde.py | 4 +- .../photonlibpy/networktables/NTTopicSet.py | 2 +- photon-lib/py/photonlibpy/packet.py | 7 +- photon-lib/py/photonlibpy/photonCamera.py | 15 +- .../py/photonlibpy/photonPoseEstimator.py | 7 +- .../photonlibpy/simulation/photonCameraSim.py | 15 +- .../simulation/simCameraProperties.py | 2 +- .../photonlibpy/simulation/visionSystemSim.py | 28 +- .../photonlibpy/simulation/visionTargetSim.py | 2 +- .../targeting/multiTargetPNPResult.py | 2 +- .../targeting/photonTrackedTarget.py | 2 +- .../py/photonlibpy/timesync/timeSyncServer.py | 2 +- photon-lib/py/setup.py | 10 +- photon-lib/py/test/openCVHelp_test.py | 2 +- .../py/test/photonPoseEstimator_test.py | 2 +- photon-lib/py/test/photonlibpy_test.py | 2 +- .../py/test/simCameraProperties_test.py | 2 +- photon-lib/py/test/visionSystemSim_test.py | 2 +- photon-lib/src/generate/photonlib.json.in | 2 +- .../java/org/photonvision/PhotonCamera.java | 37 +-- .../org/photonvision/PhotonPoseEstimator.java | 6 +- .../simulation/PhotonCameraSim.java | 4 +- .../simulation/SimCameraProperties.java | 4 +- .../simulation/VisionSystemSim.java | 16 +- .../main/native/cpp/photon/PhotonCamera.cpp | 58 +--- .../cpp/photon/simulation/PhotonCameraSim.cpp | 2 +- .../src/main/native/include/PhotonVersion.h | 1 - .../photon/simulation/VisionSystemSim.h | 16 +- .../org/photonvision/PhotonCameraTest.java | 2 +- photon-targeting/build.gradle | 4 +- .../common/hardware/Platform.java | 4 +- .../wrap/casadi_wrapper.cpp | 6 +- .../src/test/java/jni/CscoreExtrasTest.java | 4 +- .../aimandrange/build.gradle | 4 +- .../src/main/include/subsystems/SwerveDrive.h | 4 +- .../aimattarget/build.gradle | 4 +- .../src/main/include/subsystems/SwerveDrive.h | 4 +- photonlib-cpp-examples/build.gradle | 2 + photonlib-cpp-examples/poseest/build.gradle | 4 +- .../src/main/include/subsystems/SwerveDrive.h | 4 +- .../aimandrange/build.gradle | 6 +- .../aimattarget/build.gradle | 6 +- photonlib-java-examples/build.gradle | 2 + photonlib-java-examples/poseest/build.gradle | 6 +- .../subsystems/drivetrain/SwerveDrive.java | 14 +- .../aimandrange/drivetrain.py | 93 +++--- .../aimandrange/robot.py | 6 +- .../aimandrange/swervemodule.py | 76 +++-- .../aimattarget/drivetrain.py | 93 +++--- .../aimattarget/robot.py | 6 +- .../aimattarget/swervemodule.py | 76 +++-- .../poseest/drivetrain.py | 111 ++++---- photonlib-python-examples/poseest/robot.py | 14 +- .../poseest/swervemodule.py | 87 +++--- photonlib-python-examples/run.bat | 1 + photonlib-python-examples/run.sh | 2 + shared/PhotonVersion.cpp.in | 1 - shared/common.gradle | 4 +- shared/config.gradle | 3 - shared/javacommon.gradle | 4 +- versioningHelper.gradle | 42 --- 97 files changed, 732 insertions(+), 1139 deletions(-) delete mode 100644 photon-core/src/main/java/org/photonvision/common/util/file/AprilTagFieldLayoutJsonAdapter.java delete mode 100644 photon-core/src/main/java/org/photonvision/common/util/file/Pose3dJsonAdapter.java delete mode 100644 photon-core/src/main/java/org/photonvision/common/util/file/QuaternionMixIn.java delete mode 100644 photon-core/src/main/java/org/photonvision/common/util/file/Rotation3dMixIn.java delete mode 100644 photon-core/src/main/java/org/photonvision/common/util/file/Translation3dMixIn.java delete mode 100644 versioningHelper.gradle diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index dad5142bd..aef62f5b1 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -23,73 +23,73 @@ jobs: # build-examples: - # strategy: - # fail-fast: false - # matrix: - # include: - # - os: windows-2022 - # artifact-name: Win64 - # - os: macos-14 - # artifact-name: macOS - # - os: ubuntu-24.04 - # artifact-name: Linux + # strategy: + # fail-fast: false + # matrix: + # include: + # - os: windows-2022 + # artifact-name: Win64 + # - os: macos-14 + # artifact-name: macOS + # - os: ubuntu-24.04 + # artifact-name: Linux - # name: "Photonlib - Build Examples - ${{ matrix.os }}" - # runs-on: ${{ matrix.os }} - # needs: [build-photonlib-host, build-photonlib-docker] + # name: "Photonlib - Build Examples - ${{ matrix.os }}" + # runs-on: ${{ matrix.os }} + # needs: [build-photonlib-host, build-photonlib-docker] - # steps: - # - name: Checkout code - # uses: actions/checkout@v6 - # with: - # fetch-depth: 0 - # - name: Fetch tags - # run: git fetch --tags --force - # - uses: actions/setup-java@v5 - # with: - # java-version: 25 - # distribution: temurin - # - name: Install SystemCore Toolchain - # run: ./gradlew installSystemCoreToolchain - # - name: Delete duplicate toolchains - # run: | - # find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} + - # du -h . | sort -h - # if: matrix.os == 'ubuntu-24.04' - # # Download prebuilt photonlib artifacts - # - uses: actions/download-artifact@v7 - # with: - # name: maven-${{ matrix.artifact-name }} - # - uses: actions/download-artifact@v7 - # with: - # name: maven-Athena - # - name: Move to maven local - # run: | - # mkdir -p ~/.m2/repository/ - # mv maven/org ~/.m2/repository/ - # - name: Copy vendordeps - # shell: bash - # run: | - # for vendordep_folder in photonlib-*-examples/*/; do - # # Remove trailing slash for cross-platform compatibility - # vendordep_folder="${vendordep_folder%/}" + # steps: + # - name: Checkout code + # uses: actions/checkout@v6 + # with: + # fetch-depth: 0 + # - name: Fetch tags + # run: git fetch --tags --force + # - uses: actions/setup-java@v5 + # with: + # java-version: 25 + # distribution: temurin + # - name: Install SystemCore Toolchain + # run: ./gradlew installSystemCoreToolchain + # - name: Delete duplicate toolchains + # run: | + # find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} + + # du -h . | sort -h + # if: matrix.os == 'ubuntu-24.04' + # # Download prebuilt photonlib artifacts + # - uses: actions/download-artifact@v7 + # with: + # name: maven-${{ matrix.artifact-name }} + # - uses: actions/download-artifact@v7 + # with: + # name: maven-SystemCore + # - name: Move to maven local + # run: | + # mkdir -p ~/.m2/repository/ + # mv maven/org ~/.m2/repository/ + # - name: Copy vendordeps + # shell: bash + # run: | + # for vendordep_folder in photonlib-*-examples/*/; do + # # Remove trailing slash for cross-platform compatibility + # vendordep_folder="${vendordep_folder%/}" - # # Filter for projects only - # if [ -e "$vendordep_folder/build.gradle" ]; then - # mkdir -p "$vendordep_folder/vendordeps/" - # cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/" - # fi - # done - # - name: Build Java examples - # working-directory: photonlib-java-examples - # run: | - # ./gradlew build - # ./gradlew clean - # - name: Build C++ examples - # working-directory: photonlib-cpp-examples - # run: | - # ./gradlew build - # ./gradlew clean + # # Filter for projects only + # if [ -e "$vendordep_folder/build.gradle" ]; then + # mkdir -p "$vendordep_folder/vendordeps/" + # cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/" + # fi + # done + # - name: Build Java examples + # working-directory: photonlib-java-examples + # run: | + # ./gradlew build + # ./gradlew clean + # - name: Build C++ examples + # working-directory: photonlib-cpp-examples + # run: | + # ./gradlew build + # ./gradlew clean typecheck-client: name: "Typecheck Client" @@ -237,12 +237,12 @@ jobs: fail-fast: false matrix: include: -# - os: windows-2022 -# artifact-name: Win64 - - os: macos-15 # TODO: Restore to macos-26 with WPILib alpha-6 - artifact-name: macOS - - os: ubuntu-24.04 - artifact-name: Linux + - os: windows-2022 + artifact-name: Win64 + - os: macos-26 + artifact-name: macOS + - os: ubuntu-24.04 + artifact-name: Linux name: "Photonlib - Build Host - ${{ matrix.artifact-name }}" runs-on: ${{ matrix.os }} @@ -345,7 +345,7 @@ jobs: include: - os: ubuntu-24.04 artifact-name: Linux - arch-override: linuxx64 + arch-override: linuxx86-64 - os: ubuntu-24.04 artifact-name: LinuxArm64 arch-override: linuxarm64 @@ -401,7 +401,7 @@ jobs: arch-override: macarm64 - os: macos-latest artifact-name: macOS - arch-override: macx64 + arch-override: macx86-64 runs-on: ${{ matrix.os }} name: "Build fat JAR - ${{ matrix.artifact-name }}" @@ -416,8 +416,8 @@ jobs: matrix: include: - os: windows-latest - artifact-name: Win64 - arch-override: winx64 + artifact-name: Win + arch-override: winx86-64 runs-on: ${{ matrix.os }} name: "Build fat JAR - ${{ matrix.artifact-name }}" @@ -432,10 +432,10 @@ jobs: matrix: include: - os: ubuntu-24.04 - artifact-name: photonvision-*-linuxx64.jar + artifact-name: photonvision-*-linuxx86-64.jar extraOpts: -Djdk.lang.Process.launchMechanism=vfork - os: windows-latest - artifact-name: photonvision-*-winx64.jar + artifact-name: photonvision-*-winx86-64.jar - os: macos-latest artifact-name: photonvision-*-macarm64.jar - os: ubuntu-24.04-arm diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 40df06800..a3e25099e 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -12,134 +12,136 @@ concurrency: cancel-in-progress: true jobs: -# build-py: -# runs-on: ubuntu-24.04 -# -# steps: -# - name: Checkout code -# uses: actions/checkout@v6 -# with: -# fetch-depth: 0 -# -# - name: Set up Python -# uses: actions/setup-python@v6 -# with: -# python-version: 3.14 -# -# - name: Install dependencies -# run: | -# python -m pip install --upgrade pip -# pip install setuptools wheel -# -# - name: Build wheel -# working-directory: ./photon-lib/py -# run: python setup.py sdist bdist_wheel -# -# - name: Upload artifacts -# uses: actions/upload-artifact@v7 -# with: -# name: dist -# path: ./photon-lib/py/dist/ -# -# test-py: -# needs: build-py -# runs-on: ubuntu-24.04 -# -# steps: -# - name: Checkout code -# uses: actions/checkout@v6 -# with: -# fetch-depth: 0 -# -# - name: Set up Python -# uses: actions/setup-python@v6 -# with: -# python-version: 3.14 -# -# - name: Install dependencies -# run: | -# python -m pip install --upgrade pip -# pip install pytest mypy -# -# - name: Download artifacts -# uses: actions/download-artifact@v8 -# with: -# name: dist -# path: dist/ -# -# - name: Install package -# shell: bash -# run: pip install --no-cache-dir dist/*.whl -# -# - name: Run Unit Tests -# shell: bash -# run: pytest --import-mode=importlib photon-lib/py/test/ -# -# - name: Run mypy type checking -# run: mypy --show-column-numbers --config-file photon-lib/py/pyproject.toml photon-lib -# -# build-python-examples: -# needs: build-py -# strategy: -# matrix: -# os: [ubuntu-24.04, windows-2022, macos-14] -# runs-on: ${{ matrix.os }} -# -# steps: -# -# - name: Checkout code -# uses: actions/checkout@v6 -# with: -# fetch-depth: 0 -# -# - name: Set up Python -# uses: actions/setup-python@v6 -# with: -# python-version: 3.14 -# -# - name: Install dependencies -# run: | -# python -m pip install --upgrade pip -# -# - name: Download artifacts -# uses: actions/download-artifact@v8 -# with: -# name: dist -# path: ./photon-lib/py/dist/ -# -# - name: Install PhotonLibPy package -# working-directory: ./photon-lib/py -# shell: bash -# run: | -# pip install --no-cache-dir dist/*.whl -# -# - name: Build Python examples -# working-directory: photonlib-python-examples -# shell: bash -# run: | -# for folder in */; -# do -# echo $folder -# ./run.sh $folder -# done -# -# deploy: -# needs: [test-py, build-python-examples] -# runs-on: ubuntu-24.04 -# # Only upload on tags -# if: startsWith(github.ref, 'refs/tags/v') -# -# steps: -# - name: Download artifacts -# uses: actions/download-artifact@v8 -# with: -# name: dist -# path: dist/ -# -# - name: Publish package distributions to PyPI -# uses: pypa/gh-action-pypi-publish@release/v1 -# with: -# packages-dir: ./dist/ -# -# permissions: -# id-token: write # IMPORTANT: this permission is mandatory for trusted publishing + build-py: + runs-on: ubuntu-24.04 + + steps: + - name: Checkout code + uses: actions/checkout@v6 + with: + fetch-depth: 0 + + - name: Set up Python + uses: actions/setup-python@v6 + with: + python-version: 3.14 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install setuptools wheel + + - name: Build wheel + working-directory: ./photon-lib/py + run: python setup.py sdist bdist_wheel + + - name: Upload artifacts + uses: actions/upload-artifact@v7 + with: + name: dist + path: ./photon-lib/py/dist/ + + test-py: + needs: build-py + runs-on: ubuntu-24.04 + + steps: + - name: Checkout code + uses: actions/checkout@v6 + with: + fetch-depth: 0 + + - name: Set up Python + uses: actions/setup-python@v6 + with: + python-version: 3.14 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pytest mypy + + - name: Download artifacts + uses: actions/download-artifact@v8 + with: + name: dist + path: dist/ + + - name: Install package + shell: bash + run: pip install --no-cache-dir dist/*.whl + + - name: Run Unit Tests + shell: bash + run: pytest --import-mode=importlib photon-lib/py/test/ + + - name: Run mypy type checking + run: mypy --show-column-numbers --config-file photon-lib/py/pyproject.toml photon-lib + + # build-python-examples: + # needs: build-py + # strategy: + # matrix: + # os: [ubuntu-24.04, windows-2022, macos-14] + # runs-on: ${{ matrix.os }} + + # steps: + + # - name: Checkout code + # uses: actions/checkout@v6 + # with: + # fetch-depth: 0 + + # - name: Set up Python + # uses: actions/setup-python@v6 + # with: + # python-version: 3.14 + + # - name: Install dependencies + # run: | + # python -m pip install --upgrade pip + # python -m pip install mypy + + # - name: Download artifacts + # uses: actions/download-artifact@v8 + # with: + # name: dist + # path: ./photon-lib/py/dist/ + + # - name: Install PhotonLibPy package + # working-directory: ./photon-lib/py + # shell: bash + # run: | + # pip install --no-cache-dir dist/*.whl + + # - name: Build Python examples + # working-directory: photonlib-python-examples + # shell: bash + # run: | + # for folder in */; + # do + # echo $folder + # ./run.sh $folder + # done + + deploy: + # TODO: BRING BACK PYTHON EXAMPLES + needs: [test-py] + runs-on: ubuntu-24.04 + # Only upload on tags + if: startsWith(github.ref, 'refs/tags/v') + + steps: + - name: Download artifacts + uses: actions/download-artifact@v8 + with: + name: dist + path: dist/ + + - name: Publish package distributions to PyPI + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages-dir: ./dist/ + + permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing diff --git a/README.md b/README.md index ab4a57191..a905e3980 100644 --- a/README.md +++ b/README.md @@ -32,11 +32,11 @@ You can run one of the many built in examples straight from the command line, to Note that these are case sensitive! * `-PArchOverride=foobar`: builds for a target system other than your current architecture. [Valid overrides](https://github.com/wpilibsuite/wpilib-tool-plugin/blob/main/src/main/java/edu/wpi/first/tools/NativePlatforms.java) are: - * winx64 + * winx86-64 * winarm64 - * macx64 + * macx86-64 * macarm64 - * linuxx64 + * linuxx86-64 * linuxarm64 * linuxathena - `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to diff --git a/build.gradle b/build.gradle index 3e547d659..4d3c4eba6 100644 --- a/build.gradle +++ b/build.gradle @@ -4,9 +4,9 @@ plugins { id "cpp" id "com.diffplug.spotless" version "8.1.0" id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0" - id 'org.wpilib.NativeUtils' version '2027.5.1' apply false + id 'org.wpilib.NativeUtils' version '2027.7.1' apply false id 'org.wpilib.DeployUtils' version '2027.1.0' apply false - id 'org.photonvision.tools.WpilibTools' version '3.0.0-photon' + id 'org.photonvision.tools.WpilibTools' version 'v5.0.1' 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 @@ -30,27 +30,21 @@ allprojects { ext.localMavenURL = file("$project.buildDir/outputs/maven") ext.allOutputsFolder = file("$project.buildDir/outputs") -// Configure the version number. -apply from: "versioningHelper.gradle" - ext { - wpilibVersion = "2027.0.0-alpha-4" - wpimathVersion = wpilibVersion - openCVYear = "2025" - openCVversion = "4.10.0-3" + wpilibVersion = "2027.0.0-alpha-6" + openCVversion = "2027-4.13.0-3" ejmlVersion = "0.43.1"; - jacksonVersion = "2.15.2"; avajeJsonbVersion = "3.14-RC4"; msgpackVersion = "0.9.0"; quickbufVersion = "1.3.3"; jacocoVersion = "0.8.14"; javalinVersion = "6.7.0" - libcameraDriverVersion = "dev-v2026.0.0-2-gef3d7a0" - rknnVersion = "dev-v2026.0.1-1-g89b2888" - rubikVersion = "dev-v2026.0.1-4-g13d6279" - frcYear = "2027_alpha4" - mrcalVersion = "v2027.0.1"; + libcameraDriverVersion = "v2027.0.0-alpha-1" + rknnVersion = "dev-v2026.0.1-3-g14c3ecb" + tfliteVersion = "v2027.0.2-alpha-1" + year = "2027_alpha5" + mrcalVersion = "v2027.0.2"; pubVersion = versionString isDev = pubVersion.startsWith("dev") diff --git a/docs/source/docs/advanced-installation/sw_install/linux-pc.md b/docs/source/docs/advanced-installation/sw_install/linux-pc.md index 729934427..f781eb219 100644 --- a/docs/source/docs/advanced-installation/sw_install/linux-pc.md +++ b/docs/source/docs/advanced-installation/sw_install/linux-pc.md @@ -25,7 +25,7 @@ Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/re :::{note} If your coprocessor has a 64 bit ARM based CPU architecture (OrangePi, Raspberry Pi, etc.), download the LinuxArm64.jar file. -If your coprocessor has an 64 bit x86 based CPU architecture (Mini PC, laptop, etc.), download the Linuxx64.jar file. +If your coprocessor has an 64 bit x86 based CPU architecture (Mini PC, laptop, etc.), download the Linuxx86-64.jar file. ::: :::{warning} diff --git a/docs/source/docs/advanced-installation/sw_install/windows-pc.md b/docs/source/docs/advanced-installation/sw_install/windows-pc.md index 364d27210..2302a0f03 100644 --- a/docs/source/docs/advanced-installation/sw_install/windows-pc.md +++ b/docs/source/docs/advanced-installation/sw_install/windows-pc.md @@ -16,7 +16,7 @@ PhotonVision requires a JDK installed and on the system path. **JDK 17 is needed ## Downloading the Latest Stable Release of PhotonVision -Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/releases) and download the winx64.jar file. +Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/releases) and download the winx86-64.jar file. ## Running PhotonVision diff --git a/docs/source/docs/contributing/design-descriptions/e2e-latency.md b/docs/source/docs/contributing/design-descriptions/e2e-latency.md index 7d81b59b4..83870eb0f 100644 --- a/docs/source/docs/contributing/design-descriptions/e2e-latency.md +++ b/docs/source/docs/contributing/design-descriptions/e2e-latency.md @@ -79,9 +79,9 @@ public class Robot extends TimedRobot { camera.getAllUnreadResults(); } - var t1 = Timer.getFPGATimestamp(); + var t1 = Timer.getMonotonicTimestamp(); light.set(true); - var t2 = Timer.getFPGATimestamp(); + var t2 = Timer.getMonotonicTimestamp(); for (int i = 0; i < 100; i++) { diff --git a/photon-core/build.gradle b/photon-core/build.gradle index e3ca10ba6..72fbb93f8 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -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, openCVversion) + wpilibNatives wpilibTools.deps.wpilibOpenCv(openCVversion) // These stay as implementation dependencies since they don't have native code that gets packaged implementation 'org.zeroturnaround:zt-zip:1.14' @@ -41,7 +41,7 @@ dependencies { wpilibNatives("org.photonvision:rknn_jni-jni:$rknnVersion:$jniPlatform") { transitive = false } - wpilibNatives("org.photonvision:rubik_jni-jni:$rubikVersion:$jniPlatform") { + wpilibNatives("org.photonvision:tflite_jni-jni:$tfliteVersion:$jniPlatform") { transitive = false } wpilibNatives("org.photonvision:photon-libcamera-gl-driver-jni:$libcameraDriverVersion:$jniPlatform") { @@ -49,11 +49,17 @@ dependencies { } } + if (jniPlatform == "linuxx86-64") { + wpilibNatives("org.photonvision:tflite_jni-jni:$tfliteVersion:$jniPlatform") { + transitive = false + } + } + implementation("org.photonvision:rknn_jni-java:$rknnVersion") { transitive = false } - implementation("org.photonvision:rubik_jni-java:$rubikVersion") { + implementation("org.photonvision:tflite_jni-java:$tfliteVersion") { transitive = false } diff --git a/photon-core/src/main/java/org/photonvision/common/LoadJNI.java b/photon-core/src/main/java/org/photonvision/common/LoadJNI.java index 154fe40c2..2ba82cf0b 100644 --- a/photon-core/src/main/java/org/photonvision/common/LoadJNI.java +++ b/photon-core/src/main/java/org/photonvision/common/LoadJNI.java @@ -26,7 +26,7 @@ public class LoadJNI { private static HashMap loadedMap = new HashMap<>(); public enum JNITypes { - RUBIK_DETECTOR("tensorflowlite", "tensorflowlite_c", "external_delegate", "rubik_jni"), + RUBIK_DETECTOR("tflite_jni"), RKNN_DETECTOR("rga", "rknnrt", "rknn_jni"), MRCAL("mrcal_jni"), LIBCAMERA("photonlibcamera"); diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java index 2d3ae4df7..c66fbb5ec 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java @@ -36,7 +36,7 @@ public class NTDataChangeListener { this.instance = instance; listenerID = this.instance.addListener( - watchedEntry, EnumSet.of(NetworkTableEvent.Kind.kValueAll), dataChangeConsumer); + watchedEntry, EnumSet.of(NetworkTableEvent.Kind.VALUE_ALL), dataChangeConsumer); } public void remove() { diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java index 500197239..28fec29b7 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java @@ -68,9 +68,9 @@ public class NTDriverStation { fmsTable.addListener( "FMSControlData", - EnumSet.of(Kind.kValueAll), + EnumSet.of(Kind.VALUE_ALL), (table, key, event) -> { - if (event.is(Kind.kValueAll) && event.valueData.value.isInteger()) { + if (event.is(Kind.VALUE_ALL) && event.valueData.value.isInteger()) { // Logger totally isnt thread safe but whatevs var word = NTDriverStation.getControlWord(event.valueData.value.getInteger()); 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 0e30e5996..90b918ded 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 @@ -86,11 +86,11 @@ public class NetworkTablesManager { private NetworkTablesManager() { ntInstance.addLogger( - LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages + LogMessage.INFO, LogMessage.CRITICAL, this::logNtMessage); // to hide error messages ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages ntInstance.addListener( - m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged); + m_fieldLayoutSubscriber, EnumSet.of(Kind.VALUE_ALL), this::onFieldLayoutChanged); ntDriverStation = new NTDriverStation(this.getNTInst()); @@ -127,16 +127,16 @@ public class NetworkTablesManager { private void logNtMessage(NetworkTableEvent event) { String levelmsg = "DEBUG"; LogLevel pvlevel = LogLevel.DEBUG; - if (event.logMessage.level >= LogMessage.kCritical) { + if (event.logMessage.level >= LogMessage.CRITICAL) { pvlevel = LogLevel.ERROR; levelmsg = "CRITICAL"; - } else if (event.logMessage.level >= LogMessage.kError) { + } else if (event.logMessage.level >= LogMessage.ERROR) { pvlevel = LogLevel.ERROR; levelmsg = "ERROR"; - } else if (event.logMessage.level >= LogMessage.kWarning) { + } else if (event.logMessage.level >= LogMessage.WARNING) { pvlevel = LogLevel.WARN; levelmsg = "WARNING"; - } else if (event.logMessage.level >= LogMessage.kInfo) { + } else if (event.logMessage.level >= LogMessage.INFO) { pvlevel = LogLevel.INFO; levelmsg = "INFO"; } @@ -157,16 +157,14 @@ public class NetworkTablesManager { } public void checkNtConnectState(NetworkTableEvent event) { - var isConnEvent = event.is(Kind.kConnected); - var isDisconnEvent = event.is(Kind.kDisconnected); + var isConnEvent = event.is(Kind.CONNECTED); + var isDisconnEvent = event.is(Kind.DISCONNECTED); if (isDisconnEvent) { var msg = String.format( "NT lost connection to %s:%d! (NT version %d). Will retry in background.", - event.connInfo.remote_ip, - event.connInfo.remote_port, - event.connInfo.protocol_version); + event.connInfo.remoteIp, event.connInfo.remotePort, event.connInfo.protocolVersion); logger.error(msg); HardwareManager.getInstance().setNTConnected(false); @@ -175,9 +173,7 @@ public class NetworkTablesManager { var msg = String.format( "NT connected to %s:%d! (NT version %d)", - event.connInfo.remote_ip, - event.connInfo.remote_port, - event.connInfo.protocol_version); + event.connInfo.remoteIp, event.connInfo.remotePort, event.connInfo.protocolVersion); logger.info(msg); HardwareManager.getInstance().setNTConnected(true); @@ -226,7 +222,7 @@ public class NetworkTablesManager { if (ntInstance.isConnected()) { var connections = ntInstance.getConnections(); if (connections.length > 0) { - subMap.put("address", connections[0].remote_ip + ":" + connections[0].remote_port); + subMap.put("address", connections[0].remoteIp + ":" + connections[0].remotePort); } subMap.put("clients", connections.length); } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java index 478a794de..87dd71505 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java @@ -112,7 +112,7 @@ public class TimeSyncManager { var conns = ntInstance.getConnections(); if (conns.length > 0) { - var newServer = conns[0].remote_ip; + var newServer = conns[0].remoteIp; if (!m_client.getServer().equals(newServer)) { logger.debug("Changing TimeSyncClient server to " + newServer); m_client.setServer(newServer); diff --git a/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java b/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java index fde85a883..9890e7d14 100644 --- a/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java @@ -267,8 +267,8 @@ public class NetworkUtils { // Use NT client IP address to find the interface in use if (!config.runNTServer) { var conn = NetworkTableInstance.getDefault().getConnections(); - if (conn.length > 0 && !conn[0].remote_ip.equals("127.0.0.1")) { - var addr = InetAddress.getByName(conn[0].remote_ip); + if (conn.length > 0 && !conn[0].remoteIp.equals("127.0.0.1")) { + var addr = InetAddress.getByName(conn[0].remoteIp); return formatMacAddress(NetworkInterface.getByInetAddress(addr).getHardwareAddress()); } } diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/AprilTagFieldLayoutJsonAdapter.java b/photon-core/src/main/java/org/photonvision/common/util/file/AprilTagFieldLayoutJsonAdapter.java deleted file mode 100644 index 67291a2a8..000000000 --- a/photon-core/src/main/java/org/photonvision/common/util/file/AprilTagFieldLayoutJsonAdapter.java +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.common.util.file; - -import io.avaje.json.JsonAdapter; -import io.avaje.json.JsonReader; -import io.avaje.json.JsonWriter; -import io.avaje.json.PropertyNames; -import io.avaje.jsonb.CustomAdapter; -import io.avaje.jsonb.Json; -import io.avaje.jsonb.Jsonb; -import io.avaje.jsonb.Types; -import java.util.List; -import org.wpilib.vision.apriltag.AprilTag; -import org.wpilib.vision.apriltag.AprilTagFieldLayout; - -@Json.Import(AprilTag.class) -@CustomAdapter -public class AprilTagFieldLayoutJsonAdapter implements JsonAdapter { - @Json - record FieldDimensions(double length, double width) {} - - JsonAdapter> aprilTagListJsonAdapter; - JsonAdapter fieldDimensionsJsonAdapter; - PropertyNames names; - - public AprilTagFieldLayoutJsonAdapter(Jsonb jsonb) { - aprilTagListJsonAdapter = jsonb.adapter(Types.listOf(AprilTag.class)); - fieldDimensionsJsonAdapter = jsonb.adapter(FieldDimensions.class); - names = jsonb.properties("tags", "field"); - } - - @Override - public void toJson(JsonWriter writer, AprilTagFieldLayout value) { - writer.beginObject(names); - writer.name(0); - aprilTagListJsonAdapter.toJson(writer, value.getTags()); - writer.name(1); - fieldDimensionsJsonAdapter.toJson( - writer, new FieldDimensions(value.getFieldLength(), value.getFieldWidth())); - writer.endObject(); - } - - @Override - public AprilTagFieldLayout fromJson(JsonReader reader) { - List tags = null; - FieldDimensions field = null; - - reader.beginObject(); - while (reader.hasNextField()) { - final String fieldName = reader.nextField(); - switch (fieldName) { - case "tags": - tags = aprilTagListJsonAdapter.fromJson(reader); - break; - case "field": - field = fieldDimensionsJsonAdapter.fromJson(reader); - break; - default: - reader.unmappedField(fieldName); - reader.skipValue(); - } - } - reader.endObject(); - - return new AprilTagFieldLayout(tags, field.length, field.width); - } -} diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/Pose3dJsonAdapter.java b/photon-core/src/main/java/org/photonvision/common/util/file/Pose3dJsonAdapter.java deleted file mode 100644 index 60f1146db..000000000 --- a/photon-core/src/main/java/org/photonvision/common/util/file/Pose3dJsonAdapter.java +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.common.util.file; - -import io.avaje.json.JsonAdapter; -import io.avaje.json.JsonReader; -import io.avaje.json.JsonWriter; -import io.avaje.json.PropertyNames; -import io.avaje.json.view.ViewBuilder; -import io.avaje.json.view.ViewBuilderAware; -import io.avaje.jsonb.CustomAdapter; -import io.avaje.jsonb.Jsonb; -import java.lang.invoke.MethodHandle; -import org.wpilib.math.geometry.Pose3d; -import org.wpilib.math.geometry.Rotation3d; -import org.wpilib.math.geometry.Translation3d; - -@CustomAdapter -public class Pose3dJsonAdapter implements JsonAdapter, ViewBuilderAware { - private final JsonAdapter translation3dJsonAdapter; - private final JsonAdapter rotation3dJsonAdapter; - private final PropertyNames names; - - public Pose3dJsonAdapter(Jsonb jsonb) { - this.translation3dJsonAdapter = jsonb.adapter(Translation3d.class); - this.rotation3dJsonAdapter = jsonb.adapter(Rotation3d.class); - this.names = jsonb.properties("translation", "rotation"); - } - - @Override - public void toJson(JsonWriter writer, Pose3d value) { - writer.beginObject(names); - writer.name(0); - translation3dJsonAdapter.toJson(writer, value.getTranslation()); - writer.name(1); - rotation3dJsonAdapter.toJson(writer, value.getRotation()); - writer.endObject(); - } - - @Override - public Pose3d fromJson(JsonReader reader) { - Translation3d translation = null; - Rotation3d rotation = null; - - reader.beginObject(names); - while (reader.hasNextField()) { - final String fieldName = reader.nextField(); - switch (fieldName) { - case "translation": - translation = translation3dJsonAdapter.fromJson(reader); - break; - case "rotation": - rotation = rotation3dJsonAdapter.fromJson(reader); - break; - default: - reader.unmappedField(fieldName); - reader.skipValue(); - } - } - reader.endObject(); - - return new Pose3d(translation, rotation); - } - - @Override - public boolean isViewBuilderAware() { - return true; - } - - @Override - public ViewBuilderAware viewBuild() { - return this; - } - - @Override - public void build(ViewBuilder builder, String name, MethodHandle handle) { - builder.beginObject(name, handle); - builder.add( - "translation", - translation3dJsonAdapter, - builder.method(Pose3d.class, "getTranslation", Translation3d.class)); - builder.add( - "rotation", - rotation3dJsonAdapter, - builder.method(Pose3d.class, "getRotation", Rotation3d.class)); - builder.endObject(); - } -} diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/QuaternionMixIn.java b/photon-core/src/main/java/org/photonvision/common/util/file/QuaternionMixIn.java deleted file mode 100644 index 965df9452..000000000 --- a/photon-core/src/main/java/org/photonvision/common/util/file/QuaternionMixIn.java +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.common.util.file; - -import io.avaje.jsonb.Json; -import org.wpilib.math.geometry.Quaternion; - -@Json.MixIn(Quaternion.class) -public class QuaternionMixIn { - @Json.Ignore(deserialize = true) - @Json.Property("W") - double m_w; - - @Json.Ignore(deserialize = true) - @Json.Property("X") - double m_x; - - @Json.Ignore(deserialize = true) - @Json.Property("Y") - double m_y; - - @Json.Ignore(deserialize = true) - @Json.Property("Z") - double m_z; - - @Json.Creator - public static Quaternion construct(double m_w, double m_x, double m_y, double m_z) { - return new Quaternion(m_w, m_x, m_y, m_z); - } -} diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/Rotation3dMixIn.java b/photon-core/src/main/java/org/photonvision/common/util/file/Rotation3dMixIn.java deleted file mode 100644 index f89b594f3..000000000 --- a/photon-core/src/main/java/org/photonvision/common/util/file/Rotation3dMixIn.java +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.common.util.file; - -import io.avaje.jsonb.Json; -import org.wpilib.math.geometry.Quaternion; -import org.wpilib.math.geometry.Rotation3d; - -@Json.MixIn(Rotation3d.class) -public class Rotation3dMixIn { - @Json.Ignore(deserialize = true) - @Json.Property("quaternion") - Quaternion m_q; - - @Json.Creator - public static Rotation3d construct(Quaternion m_q) { - return new Rotation3d(m_q); - } -} diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/Translation3dMixIn.java b/photon-core/src/main/java/org/photonvision/common/util/file/Translation3dMixIn.java deleted file mode 100644 index 80e3e77bf..000000000 --- a/photon-core/src/main/java/org/photonvision/common/util/file/Translation3dMixIn.java +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.common.util.file; - -import io.avaje.jsonb.Json; -import org.wpilib.math.geometry.Translation3d; - -@Json.MixIn(Translation3d.class) -public class Translation3dMixIn { - @Json.Ignore(deserialize = true) - @Json.Property("x") - double m_x; - - @Json.Ignore(deserialize = true) - @Json.Property("y") - double m_y; - - @Json.Ignore(deserialize = true) - @Json.Property("z") - double m_z; - - @Json.Creator - public static Translation3d construct(double m_x, double m_y, double m_z) { - return new Translation3d(m_x, m_y, m_z); - } -} diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 8a2937cc7..bba25339e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -109,7 +109,7 @@ public class FileVisionSource extends VisionSource { this.frameStaticProperties = frameStaticProperties; videoMode = new VideoMode( - PixelFormat.kMJPEG, + PixelFormat.MJPEG, frameStaticProperties.imageWidth, frameStaticProperties.imageHeight, 30); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java index 9992d7641..2bc2ced44 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java @@ -289,8 +289,8 @@ public class GenericUSBCameraSettables extends VisionSourceSettables { try { for (VideoMode videoMode : camera.enumerateVideoModes()) { // Filter grey modes - if (videoMode.pixelFormat == PixelFormat.kGray - || videoMode.pixelFormat == PixelFormat.kUnknown) { + if (videoMode.pixelFormat == PixelFormat.GRAY + || videoMode.pixelFormat == PixelFormat.UNKNOWN) { continue; } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java index c9d3666a0..e910fb86f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java @@ -75,18 +75,18 @@ public class LibcameraGpuSettables extends VisionSourceSettables { if (sensorModel == LibCameraJNI.SensorModel.IMX219) { // Settings for the IMX219 sensor, which is used on the Pi Camera Module v2 - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 120, 120, .39)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 30, 30, .39)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 65, 90, .39)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 30, 30, .39)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 120, 120, .39)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 30, 30, .39)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 65, 90, .39)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 30, 30, .39)); // TODO: fix 1280x720 in the native code and re-add it - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, .53)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 2, 2464 / 2, 15, 20, 1)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 4, 2464 / 4, 15, 20, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1920, 1080, 15, 20, .53)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 3280 / 2, 2464 / 2, 15, 20, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 3280 / 4, 2464 / 4, 15, 20, 1)); } else if (sensorModel == LibCameraJNI.SensorModel.OV9281) { // Taken from https://www.ovt.com/wp-content/uploads/2022/01/OV9281-OV9282-PB-v1.3-WEB.pdf - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 400, 120, 240, 1)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 800, 120, 120, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 400, 120, 240, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1280, 800, 120, 120, 1)); } else { if (sensorModel == LibCameraJNI.SensorModel.IMX477) { @@ -101,13 +101,13 @@ public class LibcameraGpuSettables extends VisionSourceSettables { } // Settings for the OV5647 sensor, which is used by the Pi Camera Module v1 - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 90, 90, 1)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 85, 90, 1)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 960, 720, 45, 49, 0.74)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 90, 90, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 85, 90, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 960, 720, 45, 49, 0.74)); // Half the size of the active areas on the OV5647 - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 2592 / 2, 1944 / 2, 20, 20, 1)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 720, 30, 45, 0.91)); - videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, 0.72)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 2592 / 2, 1944 / 2, 20, 20, 1)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1280, 720, 30, 45, 0.91)); + videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1920, 1080, 15, 20, 0.72)); } // TODO need to add more video modes for new sensors here diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java index 56d1ab7f2..19344b3f8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java @@ -29,7 +29,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.frame.StaticFrames; import org.photonvision.vision.opencv.CVMat; -import org.wpilib.driverstation.DriverStation.MatchType; +import org.wpilib.driverstation.MatchType; import org.wpilib.networktables.IntegerEntry; import org.wpilib.networktables.IntegerSubscriber; import org.wpilib.networktables.NetworkTable; @@ -165,7 +165,7 @@ public class FileSaveFrameConsumer implements Consumer { logger.warn("Did not receive event name, defaulting to 'UNKNOWN'"); } - MatchType wpiMatchType = MatchType.None; // Default is to be unknown + MatchType wpiMatchType = MatchType.NONE; // Default is to be unknown if (matchType.value < 0 || matchType.value >= MatchType.values().length) { logger.error("Invalid match type from FMS: " + matchType.value); } else { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java index 167634eec..6c3969248 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java @@ -33,7 +33,7 @@ public class MJPGFrameConsumer implements AutoCloseable { private MjpegServer mjpegServer; public MJPGFrameConsumer(String sourceName, int width, int height, int port) { - this.cvSource = new CvSource(sourceName, PixelFormat.kMJPEG, width, height, 30); + this.cvSource = new CvSource(sourceName, PixelFormat.MJPEG, width, height, 30); this.mjpegServer = new MjpegServer("serve_" + cvSource.getName(), port); mjpegServer.setSource(cvSource); 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 193b4c956..c5196caa8 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 @@ -91,7 +91,7 @@ public class USBFrameProvider extends CpuImageProcessor { cameraMode.height, // hard-coded 3 channel cameraMode.width * 3, - PixelFormat.kBGR); + PixelFormat.BGR); // This is from wpi::nt::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since // Hal::initialize was called diff --git a/photon-core/src/main/java/org/photonvision/vision/objects/RubikObjectDetector.java b/photon-core/src/main/java/org/photonvision/vision/objects/RubikObjectDetector.java index 60960d8bd..bc8aea13a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/objects/RubikObjectDetector.java +++ b/photon-core/src/main/java/org/photonvision/vision/objects/RubikObjectDetector.java @@ -26,7 +26,8 @@ import org.opencv.core.Size; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ColorHelper; -import org.photonvision.rubik.RubikJNI; +import org.photonvision.tflite.TFLiteJNI; +import org.photonvision.tflite.TFLiteJNI.TFLiteSource; import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult; /** Manages an object detector using the rubik backend. */ @@ -38,7 +39,7 @@ public class RubikObjectDetector implements ObjectDetector { private final Cleanable cleanable; private static Runnable cleanupAction(long ptr) { - return () -> RubikJNI.destroy(ptr); + return () -> TFLiteJNI.destroy(ptr); } /** Pointer to the native object */ @@ -68,8 +69,10 @@ public class RubikObjectDetector implements ObjectDetector { // Create the detector try { ptr = - RubikJNI.create( - model.modelFile.getPath().toString(), model.properties.version().ordinal()); + TFLiteJNI.create( + model.modelFile.getPath().toString(), + model.properties.version().ordinal(), + TFLiteSource.RUBIK.value()); } catch (Exception e) { logger.error("Failed to create detector from path " + model.modelFile.getPath(), e); throw new RuntimeException( @@ -83,7 +86,7 @@ public class RubikObjectDetector implements ObjectDetector { + ". Please ensure the model is valid and compatible with the Rubik backend."); throw new RuntimeException( "Failed to create detector from path " + model.modelFile.getPath()); - } else if (!RubikJNI.isQuantized(ptr)) { + } else if (!TFLiteJNI.isQuantized(ptr)) { throw new UnsupportedOperationException("Model must be quantized."); } @@ -131,7 +134,7 @@ public class RubikObjectDetector implements ObjectDetector { } // Detect objects in the letterboxed frame - var results = RubikJNI.detect(ptr, letterboxed.getNativeObjAddr(), boxThresh, nmsThresh); + var results = TFLiteJNI.detect(ptr, letterboxed.getNativeObjAddr(), boxThresh, nmsThresh); letterboxed.release(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java index 0c2af2c78..b46cb519c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java @@ -31,10 +31,10 @@ public class CalculateFPSPipe @Override protected Integer process(Void in) { if (lastTime < 0) { - lastTime = Timer.getFPGATimestamp(); + lastTime = Timer.getMonotonicTimestamp(); } - var now = Timer.getFPGATimestamp(); + var now = Timer.getMonotonicTimestamp(); var dtSeconds = now - lastTime; lastTime = now; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 6ae1b8a9f..a985b34c1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -604,9 +604,8 @@ public class VisionModule { internalMap.put( "pixelFormat", ((videoMode instanceof LibcameraGpuSource.FPSRatedVideoMode) - ? "kPicam" - : videoMode.pixelFormat.toString()) - .substring(1)); // Remove the k prefix + ? "Picam" + : videoMode.pixelFormat.toString())); temp.add(internalMap); } diff --git a/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java b/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java index 5c2f59603..fed3a6e32 100644 --- a/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java @@ -36,8 +36,8 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.util.TestUtils; import org.photonvision.jni.LibraryLoader; import org.photonvision.vision.frame.provider.FileFrameProvider; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.driverstation.DriverStation.MatchType; +import org.wpilib.driverstation.MatchType; +import org.wpilib.driverstation.internal.DriverStationBackend; import org.wpilib.hardware.hal.HAL; import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.simulation.DriverStationSim; @@ -103,7 +103,7 @@ public class FileSaveFrameConsumerTest { DriverStationSim.setMatchType(matchType); DriverStationSim.setMatchNumber(matchNumber); DriverStationSim.setEventName(eventName); - DriverStation.refreshData(); + DriverStationBackend.refreshData(); // WHEN we save the image var currentTime = new Date(); diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 82e272cd3..0eb5f24e8 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -17,8 +17,6 @@ apply plugin: 'org.wpilib.NativeUtils' apply from: "${rootDir}/shared/config.gradle" apply from: "${rootDir}/shared/javacommon.gradle" -apply from: "${rootDir}/versioningHelper.gradle" - nativeUtils { exportsConfigs { "${nativeName}" {} @@ -159,7 +157,7 @@ task generateVendorJson() { def read = photonlibFileInput.text .replace('${photon_version}', pubVersion) - .replace('${frc_year}', frcYear) + .replace('${year}', year) photonlibFileOutput.text = read outputs.upToDateWhen { false } @@ -375,4 +373,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, openCVversion) +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv(openCVversion) diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py index b964bf423..3ad7e1809 100644 --- a/photon-lib/py/photonlibpy/estimatedRobotPose.py +++ b/photon-lib/py/photonlibpy/estimatedRobotPose.py @@ -17,7 +17,7 @@ from dataclasses import dataclass -from wpimath.geometry import Pose3d +from wpimath import Pose3d from .targeting.photonTrackedTarget import PhotonTrackedTarget diff --git a/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py b/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py index a084fdd10..a2c40dbf7 100644 --- a/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py +++ b/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py @@ -1,6 +1,6 @@ import math -from wpimath.geometry import Pose3d, Rotation2d, Transform3d +from wpimath import Pose3d, Rotation2d, Transform3d from wpimath.units import meters diff --git a/photon-lib/py/photonlibpy/estimation/openCVHelp.py b/photon-lib/py/photonlibpy/estimation/openCVHelp.py index 7b1de9e23..a5a56b251 100644 --- a/photon-lib/py/photonlibpy/estimation/openCVHelp.py +++ b/photon-lib/py/photonlibpy/estimation/openCVHelp.py @@ -4,7 +4,7 @@ from typing import Any import cv2 as cv import numpy as np -from wpimath.geometry import Rotation3d, Transform3d, Translation3d +from wpimath import Rotation3d, Transform3d, Translation3d from ..targeting import PnpResult, TargetCorner from .rotTrlTransform3d import RotTrlTransform3d @@ -26,7 +26,7 @@ class OpenCVHelp: @staticmethod def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d: - return -NWU_TO_EDN + (rot + NWU_TO_EDN) + return NWU_TO_EDN.inverse().rotateBy(rot.rotateBy(NWU_TO_EDN)) @staticmethod def translationToTVec(translations: list[Translation3d]) -> np.ndarray: @@ -147,7 +147,7 @@ class OpenCVHelp: in NWU, this would be {0, 0, 1} in EDN. """ - return -EDN_TO_NWU + (rot + EDN_TO_NWU) + return EDN_TO_NWU.inverse().rotateBy(rot.rotateBy(EDN_TO_NWU)) @staticmethod def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d: diff --git a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py index e4c99c394..d602ee8bf 100644 --- a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py +++ b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py @@ -1,6 +1,6 @@ from typing import Self -from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d +from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d class RotTrlTransform3d: @@ -22,7 +22,7 @@ class RotTrlTransform3d: def inverse(self) -> Self: """The inverse of this transformation. Applying the inverse will "undo" this transformation.""" - invRot = -self.rot + invRot = self.rot.inverse() invTrl = -(self.trl.rotateBy(invRot)) return type(self)(invRot, invTrl) @@ -42,7 +42,7 @@ class RotTrlTransform3d: return trlToApply.rotateBy(self.rot) + self.trl def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d: - return rotToApply + self.rot + return rotToApply.rotateBy(self.rot) def applyPose(self, poseToApply: Pose3d) -> Pose3d: return Pose3d( @@ -68,7 +68,9 @@ class RotTrlTransform3d: @classmethod def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self: return cls( - last.rotation() - initial.rotation(), + last.rotation().relativeTo(initial.rotation()), last.translation() - - initial.translation().rotateBy(last.rotation() - initial.rotation()), + - initial.translation().rotateBy( + last.rotation().relativeTo(initial.rotation()) + ), ) diff --git a/photon-lib/py/photonlibpy/estimation/targetModel.py b/photon-lib/py/photonlibpy/estimation/targetModel.py index 61ee54b79..3836b817f 100644 --- a/photon-lib/py/photonlibpy/estimation/targetModel.py +++ b/photon-lib/py/photonlibpy/estimation/targetModel.py @@ -1,7 +1,7 @@ import math from typing import List, Self -from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d +from wpimath import Pose3d, Rotation2d, Rotation3d, Translation3d from wpimath.units import meters from . import RotTrlTransform3d diff --git a/photon-lib/py/photonlibpy/estimation/visionEstimation.py b/photon-lib/py/photonlibpy/estimation/visionEstimation.py index 3ab7da4a1..6882fc0df 100644 --- a/photon-lib/py/photonlibpy/estimation/visionEstimation.py +++ b/photon-lib/py/photonlibpy/estimation/visionEstimation.py @@ -1,6 +1,6 @@ import numpy as np from robotpy_apriltag import AprilTag, AprilTagFieldLayout -from wpimath.geometry import Pose3d, Transform3d, Translation3d +from wpimath import Pose3d, Transform3d, Translation3d from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner from . import OpenCVHelp, TargetModel diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index 885ce2c96..9bb1a715d 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py @@ -49,7 +49,9 @@ class PhotonPipelineResultSerde: ret = Packet() # metadata is of non-intrinsic type PhotonPipelineMetadata - ret.encodeBytes(PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()) + ret.encodeBytes( + PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData() + ) # targets is a custom VLA! ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct) diff --git a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py index 242212e9e..4d4569978 100644 --- a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py +++ b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py @@ -1,5 +1,5 @@ import ntcore as nt -from wpimath.geometry import Transform3d +from wpimath import Transform3d from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 8aae95062..77ab756eb 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -19,7 +19,7 @@ import struct from typing import Generic, Optional, Protocol, TypeVar import wpilib -from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d +from wpimath import Quaternion, Rotation3d, Transform3d, Translation3d T = TypeVar("T") @@ -57,17 +57,16 @@ class Packet: """ def _getNextByteAsInt(self) -> int: - retVal = 0x00 - if not self.outOfBytes: try: retVal = 0x00FF & self.packetData[self.readPos] self.readPos += 1 + return retVal except IndexError: wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True) self.outOfBytes = True - return retVal + return 0x00 def getData(self) -> bytes: """ diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index cd792d763..8e868f9bd 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -118,13 +118,16 @@ class PhotonCamera: ) self._prevHeartbeat = 0 - self._prevHeartbeatChangeTime = Timer.getFPGATimestamp() + self._prevHeartbeatChangeTime = Timer.getMonotonicTimestamp() # Start the time sync server inst.start() # Usage reporting - hal.reportUsage("PhotonVision/PhotonCamera", PhotonCamera.instance_count, "") + hal.reportUsage( + "PhotonVision/PhotonCamera", # Not 100% sure if this is correct + str(PhotonCamera.instance_count), + ) PhotonCamera.instance_count += 1 def getAllUnreadResults(self) -> List[PhotonPipelineResult]: @@ -169,7 +172,7 @@ class PhotonCamera: self._versionCheck() - now = RobotController.getFPGATime() + now = RobotController.getMonotonicTime() packetWithTimestamp = self._rawBytesEntry.getAtomic() byteList = packetWithTimestamp.value packetWithTimestamp.time @@ -297,7 +300,7 @@ class PhotonCamera: """ curHeartbeat = self._heartbeatEntry.get() - now = Timer.getFPGATimestamp() + now = Timer.getMonotonicTimestamp() if curHeartbeat != self._prevHeartbeat: self._prevHeartbeat = curHeartbeat @@ -311,10 +314,10 @@ class PhotonCamera: if not _VERSION_CHECK_ENABLED: return - if (Timer.getFPGATimestamp() - _lastVersionTimeCheck) < 5.0: + if (Timer.getMonotonicTimestamp() - _lastVersionTimeCheck) < 5.0: return - _lastVersionTimeCheck = Timer.getFPGATimestamp() + _lastVersionTimeCheck = Timer.getMonotonicTimestamp() # Heartbeat entry is assumed to always be present. If it's not present, we # assume that a camera with that name was never connected in the first place. diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index ef10dbee5..f51da4018 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -21,16 +21,16 @@ import hal import wpilib import wpimath.units from robotpy_apriltag import AprilTagFieldLayout -from wpimath.geometry import ( +from wpimath import ( Pose2d, Pose3d, Rotation2d, Rotation3d, + TimeInterpolatableRotation2dBuffer, Transform3d, Translation2d, Translation3d, ) -from wpimath.interpolation import TimeInterpolatableRotation2dBuffer from .estimatedRobotPose import EstimatedRobotPose from .targeting.photonPipelineResult import PhotonPipelineResult @@ -67,7 +67,8 @@ class PhotonPoseEstimator: # Usage reporting hal.reportUsage( - "PhotonVision/PhotonPoseEstimator", PhotonPoseEstimator.instance_count, "" + "PhotonVision/PhotonPoseEstimator", + str(PhotonPoseEstimator.instance_count), ) PhotonPoseEstimator.instance_count += 1 diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index b804d3e0f..a7638d0cf 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -6,8 +6,9 @@ import cv2 as cv import numpy as np import wpilib from robotpy_apriltag import AprilTagField, AprilTagFieldLayout -from wpimath.geometry import Pose3d, Transform3d +from wpimath import Pose3d, Transform3d from wpimath.units import meters, seconds +from wpiutil import PixelFormat from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation from ..estimation.cameraTargetRelation import CameraTargetRelation @@ -66,7 +67,7 @@ class PhotonCameraSim: # TODO switch this back to default True when the functionality is enabled self.videoSimProcEnabled: bool = False self.heartbeatCounter: int = 0 - self.nextNtEntryTime = wpilib.Timer.getFPGATimestamp() + self.nextNtEntryTime = wpilib.Timer.getMonotonicTimestamp() self.tagLayout = tagLayout self.cam = camera @@ -76,7 +77,7 @@ class PhotonCameraSim: # TODO Check fps is right self.videoSimRaw = cs.CvSource( self.cam.getName() + "-raw", - cs.VideoMode.PixelFormat.kGray, + PixelFormat.GRAY, self.prop.getResWidth(), self.prop.getResHeight(), 1, @@ -88,7 +89,7 @@ class PhotonCameraSim: # TODO Check fps is right self.videoSimProcessed = cs.CvSource( self.cam.getName() + "-processed", - cs.VideoMode.PixelFormat.kGray, + PixelFormat.GRAY, self.prop.getResWidth(), self.prop.getResHeight(), 1, @@ -182,7 +183,7 @@ class PhotonCameraSim: ready """ # check if this camera is ready for another frame update - now = wpilib.Timer.getFPGATimestamp() + now = wpilib.Timer.getMonotonicTimestamp() timestamp = 0.0 iter = 0 # prepare next latest update @@ -434,7 +435,7 @@ class PhotonCameraSim: # put this simulated data to NT self.heartbeatCounter += 1 - publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6 + publishTimestampMicros = wpilib.Timer.getMonotonicTimestamp() * 1e6 return PhotonPipelineResult( ntReceiveTimestampMicros=int(publishTimestampMicros + 10), metadata=PhotonPipelineMetadata( @@ -461,7 +462,7 @@ class PhotonCameraSim: :param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency) """ if receiveTimestamp_us is None: - receiveTimestamp_us = wpilib.Timer.getFPGATimestamp() * 1e6 + receiveTimestamp_us = wpilib.Timer.getMonotonicTimestamp() * 1e6 receiveTimestamp_us = int(receiveTimestamp_us) self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp_us) diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index 6f58367d6..1d72d102d 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -5,7 +5,7 @@ import typing import cv2 as cv import numpy as np import numpy.typing as npt -from wpimath.geometry import Rotation2d, Rotation3d, Translation3d +from wpimath import Rotation2d, Rotation3d, Translation3d from wpimath.units import hertz, milliseconds, seconds from ..estimation import RotTrlTransform3d diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py index 7abd3c204..2d3c4d724 100644 --- a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py +++ b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py @@ -3,10 +3,9 @@ import typing import wpilib from robotpy_apriltag import AprilTagFieldLayout from wpilib import Field2d -from wpimath.geometry import Pose2d, Pose3d, Transform3d # TODO(auscompgeek): update import path when RobotPy re-exports are fixed -from wpimath.interpolation._interpolation import TimeInterpolatablePose3dBuffer +from wpimath import Pose2d, Pose3d, TimeInterpolatablePose3dBuffer, Transform3d from wpimath.units import seconds from ..estimation import TargetModel @@ -69,7 +68,7 @@ class VisionSystemSim: self.bufferLength ) self.camTrfMap[cameraSim].addSample( - wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera + wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera ) def clearCameras(self) -> None: @@ -92,7 +91,7 @@ class VisionSystemSim: def getRobotToCamera( self, cameraSim: PhotonCameraSim, - time: seconds = wpilib.Timer.getFPGATimestamp(), + time: seconds | None = None, ) -> Transform3d | None: """Get a simulated camera's position relative to the robot. If the requested camera is invalid, an empty optional is returned. @@ -102,6 +101,8 @@ class VisionSystemSim: :returns: The transform of this camera, or an empty optional if it is invalid """ + if time is None: + time = wpilib.Timer.getMonotonicTimestamp() if cameraSim in self.camTrfMap: trfBuffer = self.camTrfMap[cameraSim] sample = trfBuffer.sample(time) @@ -115,7 +116,7 @@ class VisionSystemSim: def getCameraPose( self, cameraSim: PhotonCameraSim, - time: seconds = wpilib.Timer.getFPGATimestamp(), + time: seconds | None = None, ) -> Pose3d | None: """Get a simulated camera's position on the field. If the requested camera is invalid, an empty optional is returned. @@ -124,6 +125,8 @@ class VisionSystemSim: :returns: The pose of this camera, or an empty optional if it is invalid """ + if time is None: + time = wpilib.Timer.getMonotonicTimestamp() robotToCamera = self.getRobotToCamera(cameraSim, time) if robotToCamera is None: return None @@ -147,7 +150,7 @@ class VisionSystemSim: """ if cameraSim in self.camTrfMap: self.camTrfMap[cameraSim].addSample( - wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera + wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera ) return True else: @@ -155,7 +158,7 @@ class VisionSystemSim: def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None: """Reset the transform history for this camera to just the current transform.""" - now = wpilib.Timer.getFPGATimestamp() + now = wpilib.Timer.getMonotonicTimestamp() def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool: if cameraSim in self.camTrfMap: @@ -240,14 +243,13 @@ class VisionSystemSim: currentTargets.remove(target) return removedList - def getRobotPose( - self, timestamp: seconds = wpilib.Timer.getFPGATimestamp() - ) -> Pose3d | None: + def getRobotPose(self, timestamp: seconds | None = None) -> Pose3d | None: """Get the robot pose in meters saved by the vision system at this timestamp. :param timestamp: Timestamp of the desired robot pose """ - + if timestamp is None: + timestamp = wpilib.Timer.getMonotonicTimestamp() return self.robotPoseBuffer.sample(timestamp) def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None: @@ -257,7 +259,7 @@ class VisionSystemSim: assert type(robotPose) is Pose3d self.robotPoseBuffer.clear() - self.robotPoseBuffer.addSample(wpilib.Timer.getFPGATimestamp(), robotPose) + self.robotPoseBuffer.addSample(wpilib.Timer.getMonotonicTimestamp(), robotPose) def getDebugField(self) -> Field2d: return self.dbgField @@ -280,7 +282,7 @@ class VisionSystemSim: self.dbgField.getObject(targetType).setPoses(posesToAdd) # save "real" robot poses over time - now = wpilib.Timer.getFPGATimestamp() + now = wpilib.Timer.getMonotonicTimestamp() self.robotPoseBuffer.addSample(now, robotPose) self.dbgField.setRobotPose(robotPose.toPose2d()) diff --git a/photon-lib/py/photonlibpy/simulation/visionTargetSim.py b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py index 5a5caff5a..6bb494be6 100644 --- a/photon-lib/py/photonlibpy/simulation/visionTargetSim.py +++ b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py @@ -1,7 +1,7 @@ import math from typing import overload -from wpimath.geometry import Pose3d, Translation3d +from wpimath import Pose3d, Translation3d from ..estimation.targetModel import TargetModel diff --git a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py index 985158db5..367199a3a 100644 --- a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py +++ b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py @@ -1,7 +1,7 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING, ClassVar -from wpimath.geometry import Transform3d +from wpimath import Transform3d if TYPE_CHECKING: from ..generated.MultiTargetPNPResultSerde import MultiTargetPNPResultSerde diff --git a/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py b/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py index e3e253bb9..87458f132 100644 --- a/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py +++ b/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py @@ -1,7 +1,7 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING, ClassVar -from wpimath.geometry import Transform3d +from wpimath import Transform3d from ..packet import Packet from .TargetCorner import TargetCorner diff --git a/photon-lib/py/photonlibpy/timesync/timeSyncServer.py b/photon-lib/py/photonlibpy/timesync/timeSyncServer.py index f7a860dd9..69fc838b8 100644 --- a/photon-lib/py/photonlibpy/timesync/timeSyncServer.py +++ b/photon-lib/py/photonlibpy/timesync/timeSyncServer.py @@ -53,7 +53,7 @@ class TimeSyncServer: PORT = 5810 def __init__(self, time_provider: Optional[Callable[[], int]] = None): - self.time_provider = time_provider or Timer.getFPGATimestamp + self.time_provider = time_provider or Timer.getMonotonicTimestamp self._process: Optional[threading.Thread] = None self.logger = logging.getLogger("PhotonVision-TimeSyncServer") diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py index b3b848740..556cbae9e 100644 --- a/photon-lib/py/setup.py +++ b/photon-lib/py/setup.py @@ -58,11 +58,11 @@ setup( version=versionString, install_requires=[ "numpy~=2.3", - "wpilib>=2026.2.1,<2027", - "robotpy-wpimath>=2026.2.1,<2027", - "robotpy-apriltag>=2026.2.1,<2027", - "robotpy-cscore>=2026.2.1,<2027", - "pyntcore>=2026.2.1,<2027", + "wpilib>=2027.0.0a6", + "robotpy-wpimath>=2027.0.0a6", + "robotpy-apriltag>=2027.0.0a6", + "robotpy-cscore>=2027.0.0a6", + "pyntcore>=2027.0.0a6", "opencv-python;platform_machine!='roborio'", ], description=descriptionStr, diff --git a/photon-lib/py/test/openCVHelp_test.py b/photon-lib/py/test/openCVHelp_test.py index 939bae551..2e4c52aa0 100644 --- a/photon-lib/py/test/openCVHelp_test.py +++ b/photon-lib/py/test/openCVHelp_test.py @@ -6,7 +6,7 @@ from photonlibpy.estimation import RotTrlTransform3d, TargetModel from photonlibpy.estimation.openCVHelp import OpenCVHelp from photonlibpy.photonCamera import setVersionCheckEnabled from photonlibpy.simulation import SimCameraProperties, VisionTargetSim -from wpimath.geometry import Pose3d, Rotation3d, Translation3d +from wpimath import Pose3d, Rotation3d, Translation3d @pytest.fixture(autouse=True) diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index 430258dbe..8acf7d9a6 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -28,7 +28,7 @@ from photonlibpy.targeting import ( from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult from robotpy_apriltag import AprilTag, AprilTagFieldLayout -from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d +from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d class PhotonCameraInjector(PhotonCamera): diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py index 8835c9977..f87ca0588 100644 --- a/photon-lib/py/test/photonlibpy_test.py +++ b/photon-lib/py/test/photonlibpy_test.py @@ -25,7 +25,7 @@ from photonlibpy.photonCamera import setVersionCheckEnabled def test_roundTrip(): ntcore.NetworkTableInstance.getDefault().stopServer() ntcore.NetworkTableInstance.getDefault().setServer("localhost") - ntcore.NetworkTableInstance.getDefault().startClient4("meme") + ntcore.NetworkTableInstance.getDefault().startClient("meme") camera = PhotonCamera("WPI2024") diff --git a/photon-lib/py/test/simCameraProperties_test.py b/photon-lib/py/test/simCameraProperties_test.py index 296b79e0f..c0018c801 100644 --- a/photon-lib/py/test/simCameraProperties_test.py +++ b/photon-lib/py/test/simCameraProperties_test.py @@ -4,7 +4,7 @@ import numpy as np import pytest from photonlibpy.estimation import RotTrlTransform3d from photonlibpy.simulation import SimCameraProperties -from wpimath.geometry import Rotation2d, Translation3d +from wpimath import Rotation2d, Translation3d @pytest.fixture(autouse=True) diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index 9b1280569..0ec192a4e 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -5,7 +5,7 @@ from photonlibpy.estimation import TargetModel, VisionEstimation from photonlibpy.photonCamera import PhotonCamera from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim from robotpy_apriltag import AprilTag, AprilTagFieldLayout -from wpimath.geometry import ( +from wpimath import ( Pose2d, Pose3d, Rotation2d, diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in index a1a62268e..32d8c0532 100644 --- a/photon-lib/src/generate/photonlib.json.in +++ b/photon-lib/src/generate/photonlib.json.in @@ -3,7 +3,7 @@ "name": "photonlib", "version": "${photon_version}", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "${frc_year}", + "year": "${year}", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index c07b6fa74..feb3885f8 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -34,7 +34,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.driverstation.DriverStationErrors; import org.wpilib.hardware.hal.HAL; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.Matrix; @@ -189,9 +189,12 @@ public class PhotonCamera implements AutoCloseable { verifyDependencies(); } + /** This is something we only need to check for java because of the way java packages opencv. */ static void verifyDependencies() { // spotless:off - if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) { + // WPILIB names their opencv version in the format YEAR-OPENCVVERSION-PATCH + // so we split on '-' and take the middle part to get the version number + if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion.split("-")[1])) { String bfw = """ @@ -232,8 +235,8 @@ public class PhotonCamera implements AutoCloseable { """; // spotless:on - DriverStation.reportWarning(bfw, false); - DriverStation.reportError(bfw, false); + DriverStationErrors.reportWarning(bfw, false); + DriverStationErrors.reportError(bfw, false); throw new UnsupportedOperationException(bfw); } } @@ -316,10 +319,10 @@ public class PhotonCamera implements AutoCloseable { timesyncAlert.setText(warningText); timesyncAlert.set(true); - if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { - prevTimeSyncWarnTime = Timer.getFPGATimestamp(); + if (Timer.getMonotonicTimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { + prevTimeSyncWarnTime = Timer.getMonotonicTimestamp(); - DriverStation.reportWarning( + DriverStationErrors.reportWarning( warningText + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.", false); @@ -465,7 +468,7 @@ public class PhotonCamera implements AutoCloseable { */ public boolean isConnected() { var curHeartbeat = heartbeatSubscriber.get(); - var now = Timer.getFPGATimestamp(); + var now = Timer.getMonotonicTimestamp(); if (curHeartbeat < 0) { // we have never heard from the camera @@ -518,19 +521,19 @@ public class PhotonCamera implements AutoCloseable { void verifyVersion() { if (!VERSION_CHECK_ENABLED) return; - if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; - lastVersionCheckTime = Timer.getFPGATimestamp(); + if ((Timer.getMonotonicTimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; + lastVersionCheckTime = Timer.getMonotonicTimestamp(); // Heartbeat entry is assumed to always be present. If it's not present, we // assume that a camera with that name was never connected in the first place. if (!heartbeatSubscriber.exists()) { var cameraNames = getTablesThatLookLikePhotonCameras(); if (cameraNames.isEmpty()) { - DriverStation.reportError( + DriverStationErrors.reportError( "Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", false); } else { - DriverStation.reportError( + DriverStationErrors.reportError( "PhotonVision coprocessor at path " + path + " not found on NetworkTables. Double check that your camera names match!", @@ -543,7 +546,7 @@ public class PhotonCamera implements AutoCloseable { cameraNameStr.append("\n"); } - DriverStation.reportError( + DriverStationErrors.reportError( "Found the following PhotonVision cameras on NetworkTables:\n" + cameraNameStr.toString(), false); @@ -551,7 +554,7 @@ public class PhotonCamera implements AutoCloseable { } // Check for connection status. Warn if disconnected. else if (!isConnected()) { - DriverStation.reportWarning( + DriverStationErrors.reportWarning( "PhotonVision coprocessor at path " + path + " is not sending new data.", false); } @@ -563,7 +566,7 @@ public class PhotonCamera implements AutoCloseable { if (remote_uuid == null || remote_uuid.isEmpty()) { // not connected yet? - DriverStation.reportWarning( + DriverStationErrors.reportWarning( "PhotonVision coprocessor at path " + path + " has not reported a message interface UUID - is your coprocessor's camera started?", @@ -597,7 +600,7 @@ public class PhotonCamera implements AutoCloseable { """; // spotless:on - DriverStation.reportWarning(bfw, false); + DriverStationErrors.reportWarning(bfw, false); String versionMismatchMessage = "Photon version " + PhotonVersion.versionString @@ -610,7 +613,7 @@ public class PhotonCamera implements AutoCloseable { + remote_uuid + ")" + "!"; - DriverStation.reportError(versionMismatchMessage, false); + DriverStationErrors.reportError(versionMismatchMessage, false); throw new UnsupportedOperationException(versionMismatchMessage); } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 49c221e0e..537b18ff4 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -29,7 +29,7 @@ import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.DriverStationErrors; import org.wpilib.hardware.hal.HAL; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Pose3d; @@ -608,7 +608,7 @@ public class PhotonPoseEstimator { return Optional.empty(); } if (referencePose == null) { - DriverStation.reportError( + DriverStationErrors.reportError( "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", false); return Optional.empty(); @@ -760,7 +760,7 @@ public class PhotonPoseEstimator { private void reportFiducialPoseError(int fiducialId) { if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( + DriverStationErrors.reportError( "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); reportedErrors.add(fiducialId); } 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 1b7eba56f..ebbe59cb2 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -153,7 +153,7 @@ public class PhotonCameraSim implements AutoCloseable { videoSimRaw = CameraServer.putVideo(camera.getName() + "-raw", prop.getResWidth(), prop.getResHeight()); - videoSimRaw.setPixelFormat(PixelFormat.kGray); + videoSimRaw.setPixelFormat(PixelFormat.GRAY); videoSimProcessed = CameraServer.putVideo( camera.getName() + "-processed", prop.getResWidth(), prop.getResHeight()); @@ -649,7 +649,7 @@ public class PhotonCameraSim implements AutoCloseable { } // put this simulated data to NT - var now = RobotController.getFPGATime(); + var now = RobotController.getMonotonicTime(); var ret = new PhotonPipelineResult( heartbeatCounter, diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 09f769f39..cea550d16 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -41,7 +41,7 @@ import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.RotTrlTransform3d; -import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.DriverStationErrors; import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; @@ -169,7 +169,7 @@ public class SimCameraProperties { public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) { fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179)); - DriverStation.reportError( + DriverStationErrors.reportError( "Requested invalid FOV! Clamping between (1, 179) degrees...", false); } double resDiag = Math.hypot(resWidth, resHeight); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index a2dca15cf..46491defe 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -106,7 +106,7 @@ public class VisionSystemSim { camTrfMap.put(cameraSim, TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds)); camTrfMap .get(cameraSim) - .addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera)); + .addSample(Timer.getMonotonicTimestamp(), new Pose3d().plus(robotToCamera)); } } @@ -137,7 +137,7 @@ public class VisionSystemSim { * @return The transform of this camera, or an empty optional if it is invalid */ public Optional getRobotToCamera(PhotonCameraSim cameraSim) { - return getRobotToCamera(cameraSim, Timer.getFPGATimestamp()); + return getRobotToCamera(cameraSim, Timer.getMonotonicTimestamp()); } /** @@ -164,7 +164,7 @@ public class VisionSystemSim { * @return The pose of this camera, or an empty optional if it is invalid */ public Optional getCameraPose(PhotonCameraSim cameraSim) { - return getCameraPose(cameraSim, Timer.getFPGATimestamp()); + return getCameraPose(cameraSim, Timer.getMonotonicTimestamp()); } /** @@ -191,7 +191,7 @@ public class VisionSystemSim { public boolean adjustCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) { var trfBuffer = camTrfMap.get(cameraSim); if (trfBuffer == null) return false; - trfBuffer.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera)); + trfBuffer.addSample(Timer.getMonotonicTimestamp(), new Pose3d().plus(robotToCamera)); return true; } @@ -207,7 +207,7 @@ public class VisionSystemSim { * @return If the cameraSim was valid and transforms were reset */ public boolean resetCameraTransforms(PhotonCameraSim cameraSim) { - double now = Timer.getFPGATimestamp(); + double now = Timer.getMonotonicTimestamp(); var trfBuffer = camTrfMap.get(cameraSim); if (trfBuffer == null) return false; var lastTrf = new Transform3d(new Pose3d(), trfBuffer.getSample(now).orElse(new Pose3d())); @@ -339,7 +339,7 @@ public class VisionSystemSim { * @return The latest robot pose */ public Pose3d getRobotPose() { - return getRobotPose(Timer.getFPGATimestamp()); + return getRobotPose(Timer.getMonotonicTimestamp()); } /** @@ -368,7 +368,7 @@ public class VisionSystemSim { */ public void resetRobotPose(Pose3d robotPose) { robotPoseBuffer.clear(); - robotPoseBuffer.addSample(Timer.getFPGATimestamp(), robotPose); + robotPoseBuffer.addSample(Timer.getMonotonicTimestamp(), robotPose); } public Field2d getDebugField() { @@ -403,7 +403,7 @@ public class VisionSystemSim { if (robotPoseMeters == null) return; // save "real" robot poses over time - double now = Timer.getFPGATimestamp(); + double now = Timer.getMonotonicTimestamp(); robotPoseBuffer.addSample(now, robotPoseMeters); dbgField.setRobotPose(robotPoseMeters.toPose2d()); diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index a63fe1aaf..ab2aab76a 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -46,51 +46,6 @@ static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s; static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms; -inline void verifyDependencies() { - if (!(std::string_view{cv::getVersionString()} == - std::string_view{photon::PhotonVersion::opencvTargetVersion})) { - std::string bfw = - "\n\n\n\n\n" - ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" - ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n" - ">>> \n" - ">>> You are running an incompatible version \n" - ">>> of PhotonVision ! \n" - ">>> \n" - ">>> PhotonLib "; - bfw += photon::PhotonVersion::versionString; - bfw += " is built for OpenCV "; - bfw += photon::PhotonVersion::opencvTargetVersion; - bfw += - "\n" - ">>> but you are using OpenCV "; - bfw += cv::getVersionString(); - bfw += - "\n>>> \n" - ">>> This is neither tested nor supported. \n" - ">>> You MUST update WPILib, PhotonLib, or both.\n" - ">>> Check `./gradlew dependencies` and ensure\n" - ">>> all mentions of OpenCV match the version \n" - ">>> that PhotonLib was built for. If you find a" - ">>> a mismatched version in a dependency, you\n" - ">>> must take steps to update the version of \n" - ">>> OpenCV used in that dependency. If you do\n" - ">>> not control that dependency and an updated\n" - ">>> version is not available, contact the \n" - ">>> developers of that dependency. \n" - ">>> \n" - ">>> Your code will now crash. \n" - ">>> We hope your day gets better. \n" - ">>> \n" - ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n" - ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"; - - WPILIB_ReportWarning(bfw); - WPILIB_ReportError(wpi::err::Error, bfw); - throw new std::runtime_error(std::string{bfw}); - } -} - // bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want // to avoid calling this from static initialization static void InitTspServer() { @@ -169,7 +124,6 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, "' is disconnected.", wpi::Alert::Level::MEDIUM), timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::Level::MEDIUM) { - verifyDependencies(); InstanceCount++; HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); @@ -195,7 +149,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Fill the packet with latest data and populate result. wpi::units::microsecond_t now = - wpi::units::microsecond_t(wpi::RobotController::GetFPGATime()); + wpi::units::microsecond_t(wpi::RobotController::GetMonotonicTime()); const auto value = rawBytesEntry.Get(); if (!value.size()) return PhotonPipelineResult{}; @@ -265,9 +219,9 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) { timesyncAlert.SetText(warningText); timesyncAlert.Set(true); - if (wpi::Timer::GetFPGATimestamp() < + if (wpi::Timer::GetMonotonicTimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { - prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp(); + prevTimeSyncWarnTime = wpi::Timer::GetMonotonicTimestamp(); WPILIB_ReportWarning( warningText + @@ -325,7 +279,7 @@ const std::string_view PhotonCamera::GetCameraName() const { bool PhotonCamera::IsConnected() { auto currentHeartbeat = heartbeatSubscriber.Get(); - auto now = wpi::Timer::GetFPGATimestamp(); + auto now = wpi::Timer::GetMonotonicTimestamp(); if (currentHeartbeat < 0) { // we have never heard from the camera @@ -373,10 +327,10 @@ void PhotonCamera::VerifyVersion() { return; } - if ((wpi::Timer::GetFPGATimestamp() - lastVersionCheckTime) < + if ((wpi::Timer::GetMonotonicTimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; - this->lastVersionCheckTime = wpi::Timer::GetFPGATimestamp(); + this->lastVersionCheckTime = wpi::Timer::GetMonotonicTimestamp(); const std::string& versionString = versionEntry.Get(""); if (versionString.empty()) { 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 d3f18b1c6..8be6e0bec 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::util::PixelFormat::kGray); + videoSimRaw.SetPixelFormat(wpi::util::PixelFormat::GRAY); videoSimProcessed = wpi::CameraServer::PutVideo( std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), prop.GetResHeight()); diff --git a/photon-lib/src/main/native/include/PhotonVersion.h b/photon-lib/src/main/native/include/PhotonVersion.h index e2533929a..dcfba722a 100644 --- a/photon-lib/src/main/native/include/PhotonVersion.h +++ b/photon-lib/src/main/native/include/PhotonVersion.h @@ -30,6 +30,5 @@ extern const char* versionString; extern const char* buildDate; extern const bool isRelease; extern const char* wpilibTargetVersion; -extern const char* opencvTargetVersion; } // namespace PhotonVersion } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h index 95a76326f..f0338af82 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -101,7 +101,7 @@ class VisionSystemSim { std::make_pair(std::move(cameraSim), wpi::math::TimeInterpolatableBuffer{ bufferLength})); - camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(), + camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetMonotonicTimestamp(), wpi::math::Pose3d{} + robotToCamera); } } @@ -138,7 +138,7 @@ class VisionSystemSim { */ std::optional GetRobotToCamera( PhotonCameraSim* cameraSim) { - return GetRobotToCamera(cameraSim, wpi::Timer::GetFPGATimestamp()); + return GetRobotToCamera(cameraSim, wpi::Timer::GetMonotonicTimestamp()); } /** @@ -175,7 +175,7 @@ class VisionSystemSim { * @return The pose of this camera, or an empty optional if it is invalid */ std::optional GetCameraPose(PhotonCameraSim* cameraSim) { - return GetCameraPose(cameraSim, wpi::Timer::GetFPGATimestamp()); + return GetCameraPose(cameraSim, wpi::Timer::GetMonotonicTimestamp()); } /** @@ -207,7 +207,7 @@ class VisionSystemSim { bool AdjustCamera(PhotonCameraSim* cameraSim, const wpi::math::Transform3d& robotToCamera) { if (camTrfMap.find(cameraSim) != camTrfMap.end()) { - camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(), + camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetMonotonicTimestamp(), wpi::math::Pose3d{} + robotToCamera); return true; } else { @@ -230,7 +230,7 @@ class VisionSystemSim { * @return If the cameraSim was valid and transforms were reset */ bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { - wpi::units::second_t now = wpi::Timer::GetFPGATimestamp(); + wpi::units::second_t now = wpi::Timer::GetMonotonicTimestamp(); if (camTrfMap.find(cameraSim) != camTrfMap.end()) { auto trfBuffer = camTrfMap.at(cameraSim); wpi::math::Transform3d lastTrf{ @@ -369,7 +369,7 @@ class VisionSystemSim { * @return The latest robot pose */ wpi::math::Pose3d GetRobotPose() { - return GetRobotPose(wpi::Timer::GetFPGATimestamp()); + return GetRobotPose(wpi::Timer::GetMonotonicTimestamp()); } /** @@ -398,7 +398,7 @@ class VisionSystemSim { */ void ResetRobotPose(const wpi::math::Pose3d& robotPose) { robotPoseBuffer.Clear(); - robotPoseBuffer.AddSample(wpi::Timer::GetFPGATimestamp(), robotPose); + robotPoseBuffer.AddSample(wpi::Timer::GetMonotonicTimestamp(), robotPose); } wpi::Field2d& GetDebugField() { return dbgField; } @@ -427,7 +427,7 @@ class VisionSystemSim { dbgField.GetObject(set.first)->SetPoses(posesToAdd); } - wpi::units::second_t now = wpi::Timer::GetFPGATimestamp(); + wpi::units::second_t now = wpi::Timer::GetMonotonicTimestamp(); robotPoseBuffer.AddSample(now, robotPose); dbgField.SetRobotPose(robotPose.ToPose2d()); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 04cb0364d..2edbed5a3 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -125,7 +125,7 @@ class PhotonCameraTest { var res = camera.getLatestResult(); var captureTime = res.getTimestampSeconds(); - var now = Timer.getFPGATimestamp(); + var now = Timer.getMonotonicTimestamp(); // expectTrue(captureTime < now); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 33cca2320..563afbc5f 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -13,8 +13,6 @@ ext.licenseFile = file("$rootDir/LICENSE") apply from: "${rootDir}/shared/config.gradle" apply from: "${rootDir}/shared/javacommon.gradle" -apply from: "${rootDir}/versioningHelper.gradle" - nativeUtils { exportsConfigs { "${nativeName}" {} @@ -187,5 +185,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, openCVversion) +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv(openCVversion) nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") diff --git a/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java index 784c4e0b0..801a42106 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java +++ b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java @@ -152,9 +152,9 @@ public enum Platform { switch (platPath) { case "/linux/x86-64/": - return "linuxx64"; + return "linuxx86-64"; case "/windows/x86-64/": - return "winx64"; + return "winx86-64"; case "/linux/arm64/": return "linuxarm64"; default: 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 e92d52442..e6b698c89 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 @@ -169,8 +169,7 @@ constrained_solvepnp::do_optimization( point_observations.cols() != (nTags * 4)) { if constexpr (VERBOSE) fmt::println("Got unexpected num cols!"); // TODO find a new error code - return wpi::util::unexpected{ - slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS}; + return wpi::util::unexpected{slp::ExitStatus::NONFINITE_INITIAL_GUESS}; } // rescale observations to homogenous pixel coordinates @@ -199,8 +198,7 @@ constrained_solvepnp::do_optimization( auto problemOpt = createProblem(nTags, heading_free); if (!problemOpt) { - return wpi::util::unexpected{ - slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS}; + return wpi::util::unexpected{slp::ExitStatus::NONFINITE_INITIAL_GUESS}; } ProblemState<3> pState{robot2camera, field2points, point_observations, diff --git a/photon-targeting/src/test/java/jni/CscoreExtrasTest.java b/photon-targeting/src/test/java/jni/CscoreExtrasTest.java index 9816a5e6e..2cefdd3c0 100644 --- a/photon-targeting/src/test/java/jni/CscoreExtrasTest.java +++ b/photon-targeting/src/test/java/jni/CscoreExtrasTest.java @@ -59,7 +59,7 @@ public class CscoreExtrasTest { UsbCamera camera = CameraServer.startAutomaticCapture(2); - camera.setVideoMode(PixelFormat.kMJPEG, 1280, 720, 30); + camera.setVideoMode(PixelFormat.MJPEG, 1280, 720, 30); var cameraMode = camera.getVideoMode(); CvSink cvSink = CameraServer.getVideo(camera); @@ -74,7 +74,7 @@ public class CscoreExtrasTest { cameraMode.height, // hard-coded 3 channel cameraMode.width * 3, - PixelFormat.kBGR); + PixelFormat.BGR); final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0; long time = CscoreExtras.grabRawSinkFrameTimeoutLastTime( diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index 1eea95c6b..bc502b195 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } repositories { @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. 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 071864066..b71181c0b 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,6 +24,7 @@ #pragma once +#include #include #include #include @@ -73,8 +74,7 @@ class SwerveDrive { wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; wpi::math::ChassisSpeeds targetChassisSpeeds{}; - // TODO(Jade) onboard imu doesn't have sim yet - // wpi::sim::ADXRS450_GyroSim gyroSim; + wpi::sim::OnboardIMUSim gyroSim; SwerveDriveSim swerveDriveSim; wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 1eea95c6b..bc502b195 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } repositories { @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. 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 8422889ae..956f28486 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include @@ -75,7 +75,7 @@ class SwerveDrive { wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; wpi::math::ChassisSpeeds targetChassisSpeeds{}; - wpi::sim::ADXRS450_GyroSim gyroSim; + wpi::sim::OnboardIMUSim gyroSim; SwerveDriveSim swerveDriveSim; wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 0a174f78c..3b91f1dcf 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -1,5 +1,6 @@ plugins { id "com.diffplug.spotless" version "8.4.0" + id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0" } allprojects { @@ -8,6 +9,7 @@ allprojects { mavenCentral() mavenLocal() maven { url = "https://maven.photonvision.org/releases" } + wpilibRepositories.use2027Repos() } } diff --git a/photonlib-cpp-examples/poseest/build.gradle b/photonlib-cpp-examples/poseest/build.gradle index 1eea95c6b..bc502b195 100644 --- a/photonlib-cpp-examples/poseest/build.gradle +++ b/photonlib-cpp-examples/poseest/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } repositories { @@ -11,7 +11,7 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. 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 cc65085bb..966a16e1b 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include @@ -80,7 +80,7 @@ class SwerveDrive { wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; wpi::math::ChassisSpeeds targetChassisSpeeds{}; - wpi::sim::ADXRS450_GyroSim gyroSim; + wpi::sim::OnboardIMUSim gyroSim; SwerveDriveSim swerveDriveSim; wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 45c13ccfc..262454746 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } java { @@ -15,7 +15,7 @@ repositories { } wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -103,7 +103,7 @@ jar { from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index fc4128069..f57a6241f 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } java { @@ -11,7 +11,7 @@ java { def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -99,7 +99,7 @@ jar { from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 0211b3b9e..58695bb02 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,5 +1,6 @@ plugins { id "com.diffplug.spotless" version "8.4.0" + id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0" } allprojects { @@ -8,6 +9,7 @@ allprojects { mavenCentral() mavenLocal() maven { url = "https://maven.photonvision.org/releases" } + wpilibRepositories.use2027Repos() } } diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index fc4128069..f57a6241f 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" } java { @@ -11,7 +11,7 @@ java { def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2027.0.0-alpha-4" +wpi.versions.wpilibVersion = "2027.0.0-alpha-6" // Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -99,7 +99,7 @@ jar { from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 0b558dd3e..ce792fb54 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -41,6 +41,7 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.*; import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.simulation.OnboardIMUSim; import org.wpilib.smartdashboard.SmartDashboard; public class SwerveDrive { @@ -69,8 +70,7 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - // TODO(Jade) WPILib doesn't have onboard IMU sim yet - // private final ADXRS450_GyroSim gyroSim; + private final OnboardIMUSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public class SwerveDrive { visionStdDevs); // ----- Simulation - // gyroSim = new ADXRS450_GyroSim(gyro); + gyroSim = new OnboardIMUSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -188,8 +188,8 @@ public class SwerveDrive { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - // gyroSim.setAngle(-pose.getRotation().getDegrees()); - // gyroSim.setRate(0); + gyroSim.setYaw(-pose.getRotation().getDegrees()); + gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -312,8 +312,8 @@ public class SwerveDrive { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + gyroSim.setYaw(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-python-examples/aimandrange/drivetrain.py b/photonlib-python-examples/aimandrange/drivetrain.py index e0798d37a..d388d4655 100644 --- a/photonlib-python-examples/aimandrange/drivetrain.py +++ b/photonlib-python-examples/aimandrange/drivetrain.py @@ -28,8 +28,7 @@ import math import swervemodule import wpilib import wpilib.simulation -import wpimath.geometry -import wpimath.kinematics +import wpimath kMaxSpeed = 3.0 # 3 meters per second kMaxAngularSpeed = math.pi # 1/2 rotation per second @@ -41,10 +40,10 @@ class Drivetrain: """ def __init__(self) -> None: - self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) - self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) - self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) - self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.Translation2d(-0.381, -0.381) self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) @@ -55,16 +54,17 @@ class Drivetrain: wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField) self.gyro = wpilib.AnalogGyro(0) - self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro) + self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro) + self._yaw = 0.0 - self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.kinematics = wpimath.SwerveDrive4Kinematics( self.frontLeftLocation, self.frontRightLocation, self.backLeftLocation, self.backRightLocation, ) - self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.odometry = wpimath.SwerveDrive4Odometry( self.kinematics, self.gyro.getRotation2d(), ( @@ -75,7 +75,7 @@ class Drivetrain: ), ) - self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + self.targetChassisVelocities = wpimath.ChassisVelocities() self.gyro.reset() @@ -95,27 +95,24 @@ class Drivetrain: :param fieldRelative: Whether the provided x and y speeds are relative to the field. :param periodSeconds: Time """ - swerveModuleStates = self.kinematics.toSwerveModuleStates( - wpimath.kinematics.ChassisSpeeds.discretize( - ( - wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, self.gyro.getRotation2d() - ) - if fieldRelative - else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot) - ), - periodSeconds, + chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot) + if fieldRelative: + chassisVelocities = chassisVelocities.toRobotRelative( + self.gyro.getRotation2d() ) - ) - wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, kMaxSpeed - ) - self.frontLeft.setDesiredState(swerveModuleStates[0]) - self.frontRight.setDesiredState(swerveModuleStates[1]) - self.backLeft.setDesiredState(swerveModuleStates[2]) - self.backRight.setDesiredState(swerveModuleStates[3]) - self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates) + chassisVelocities = chassisVelocities.discretize(periodSeconds) + swerveModuleStates = self.kinematics.desaturateWheelVelocities( + self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed + ) + self.frontLeft.setDesiredVelocity(swerveModuleStates[0]) + self.frontRight.setDesiredVelocity(swerveModuleStates[1]) + self.backLeft.setDesiredVelocity(swerveModuleStates[2]) + self.backRight.setDesiredVelocity(swerveModuleStates[3]) + + self.targetChassisVelocities = self.kinematics.toChassisVelocities( + swerveModuleStates + ) def updateOdometry(self) -> None: """Updates the field relative position of the robot.""" @@ -129,26 +126,26 @@ class Drivetrain: ), ) - def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]: return [ - self.frontLeft.getState(), - self.frontRight.getState(), - self.backLeft.getState(), - self.backRight.getState(), + self.frontLeft.getVelocity(), + self.frontRight.getVelocity(), + self.backLeft.getVelocity(), + self.backRight.getVelocity(), ] - def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: + def getModulePoses(self) -> list[wpimath.Pose2d]: p = self.odometry.getPose() - flTrans = wpimath.geometry.Transform2d( + flTrans = wpimath.Transform2d( self.frontLeftLocation, self.frontLeft.getAbsoluteHeading() ) - frTrans = wpimath.geometry.Transform2d( + frTrans = wpimath.Transform2d( self.frontRightLocation, self.frontRight.getAbsoluteHeading() ) - blTrans = wpimath.geometry.Transform2d( + blTrans = wpimath.Transform2d( self.backLeftLocation, self.backLeft.getAbsoluteHeading() ) - brTrans = wpimath.geometry.Transform2d( + brTrans = wpimath.Transform2d( self.backRightLocation, self.backRight.getAbsoluteHeading() ) return [ @@ -158,8 +155,8 @@ class Drivetrain: p.transformBy(brTrans), ] - def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: - return self.kinematics.toChassisSpeeds(self.getModuleStates()) + def getChassisVelocities(self) -> wpimath.ChassisVelocities: + return self.kinematics.toChassisVelocities(self.getModuleVelocities()) def log(self): table = "Drive/" @@ -169,7 +166,7 @@ class Drivetrain: wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) - chassisSpeeds = self.getChassisSpeeds() + chassisSpeeds = self.getChassisVelocities() wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) wpilib.SmartDashboard.putNumber( @@ -177,13 +174,13 @@ class Drivetrain: ) wpilib.SmartDashboard.putNumber( - table + "Target VX", self.targetChassisSpeeds.vx + table + "Target VX", self.targetChassisVelocities.vx ) wpilib.SmartDashboard.putNumber( - table + "Target VY", self.targetChassisSpeeds.vy + table + "Target VY", self.targetChassisVelocities.vy ) wpilib.SmartDashboard.putNumber( - table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps + table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps ) self.frontLeft.log() @@ -199,5 +196,7 @@ class Drivetrain: self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() self.backRight.simulationPeriodic() - self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) - self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) + rate = -1.0 * self.getChassisVelocities().omega_dps + self.simGyro.setRate(rate) + self._yaw += rate * 0.02 + self.simGyro.setYaw(self._yaw) diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py index a91bcbbd4..083b3c97e 100644 --- a/photonlib-python-examples/aimandrange/robot.py +++ b/photonlib-python-examples/aimandrange/robot.py @@ -39,9 +39,11 @@ TAG_7_MOUNT_HEIGHT_m = 1.435 # From the 2024 game manual class MyRobot(wpilib.TimedRobot): - def robotInit(self) -> None: + def __init__(self) -> None: """Robot initialization function""" - self.controller = wpilib.XboxController(0) + super().__init__() + + self.controller = wpilib.NiDsXboxController(0) self.swerve = drivetrain.Drivetrain() self.cam = PhotonCamera("YOUR CAMERA NAME") diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py index d269c620c..526119c38 100644 --- a/photonlib-python-examples/aimandrange/swervemodule.py +++ b/photonlib-python-examples/aimandrange/swervemodule.py @@ -26,11 +26,7 @@ import math import wpilib import wpilib.simulation -import wpimath.controller -import wpimath.filter -import wpimath.geometry -import wpimath.kinematics -import wpimath.trajectory +import wpimath import wpimath.units kWheelRadius = 0.0508 @@ -60,7 +56,7 @@ class SwerveModule: :param turningEncoderChannelB: DIO input for the turning encoder channel B """ self.moduleNumber = moduleNumber - self.desiredState = wpimath.kinematics.SwerveModuleState() + self.desiredVelocity = wpimath.SwerveModuleVelocity() self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) @@ -71,14 +67,14 @@ class SwerveModule: ) # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) + self.drivePIDController = wpimath.PIDController(10, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = wpimath.controller.PIDController(30, 0, 0) + self.turningPIDController = wpimath.PIDController(30, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) - self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) + self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7) # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -99,66 +95,60 @@ class SwerveModule: # Simulation Support self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder) self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder) - self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor) - self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor) - self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.1, 0.02 - ) - self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.0001, 0.02 - ) + self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel) + self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel) + self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02) + self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02) self.simTurningEncoderPos = 0 self.simDrivingEncoderPos = 0 - def getState(self) -> wpimath.kinematics.SwerveModuleState: + def getVelocity(self) -> wpimath.SwerveModuleVelocity: """Returns the current state of the module. :returns: The current state of the module. """ - return wpimath.kinematics.SwerveModuleState( + return wpimath.SwerveModuleVelocity( self.driveEncoder.getRate(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + def getPosition(self) -> wpimath.SwerveModulePosition: """Returns the current position of the module. :returns: The current position of the module. """ - return wpimath.kinematics.SwerveModulePosition( + return wpimath.SwerveModulePosition( self.driveEncoder.getDistance(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def setDesiredState( - self, desiredState: wpimath.kinematics.SwerveModuleState - ) -> None: + def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None: """Sets the desired state for the module. - :param desiredState: Desired state with speed and angle. + :param desiredVelocity: Desired state with speed and angle. """ - self.desiredState = desiredState + self.desiredVelocity = desiredVelocity - encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance()) # Optimize the reference state to avoid spinning further than 90 degrees - desiredState.optimize(encoderRotation) + desiredVelocity.optimize(encoderRotation) # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired # direction of travel that can occur when modules change directions. This results in smoother # driving. - desiredState.speed *= (desiredState.angle - encoderRotation).cos() + desiredVelocity.velocity *= (desiredVelocity.angle - encoderRotation).cos() # Calculate the drive output from the drive PID controller. driveOutput = self.drivePIDController.calculate( - self.driveEncoder.getRate(), desiredState.speed + self.driveEncoder.getRate(), desiredVelocity.velocity ) - driveFeedforward = self.driveFeedforward.calculate(desiredState.speed) + driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity) # Calculate the turning motor output from the turning PID controller. turnOutput = self.turningPIDController.calculate( - self.turningEncoder.getDistance(), desiredState.angle.radians() + self.turningEncoder.getDistance(), desiredVelocity.angle.radians() ) turnFeedforward = self.turnFeedforward.calculate( @@ -168,11 +158,11 @@ class SwerveModule: self.driveMotor.setVoltage(driveOutput + driveFeedforward) self.turningMotor.setVoltage(turnOutput + turnFeedforward) - def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: - return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + def getAbsoluteHeading(self) -> wpimath.Rotation2d: + return wpimath.Rotation2d(self.turningEncoder.getDistance()) def log(self) -> None: - state = self.getState() + state = self.getVelocity() table = "Module " + str(self.moduleNumber) + "/" wpilib.SmartDashboard.putNumber( @@ -183,15 +173,17 @@ class SwerveModule: table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()), ) - wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps) wpilib.SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", self.desiredState.speed_fps + table + "Drive Velocity Feet", state.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Drive Voltage", self.driveMotor.get() * 12.0 + table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Steer Voltage", self.turningMotor.get() * 12.0 + table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0 + ) + wpilib.SmartDashboard.putNumber( + table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0 ) def simulationPeriodic(self) -> None: diff --git a/photonlib-python-examples/aimattarget/drivetrain.py b/photonlib-python-examples/aimattarget/drivetrain.py index e0798d37a..d388d4655 100644 --- a/photonlib-python-examples/aimattarget/drivetrain.py +++ b/photonlib-python-examples/aimattarget/drivetrain.py @@ -28,8 +28,7 @@ import math import swervemodule import wpilib import wpilib.simulation -import wpimath.geometry -import wpimath.kinematics +import wpimath kMaxSpeed = 3.0 # 3 meters per second kMaxAngularSpeed = math.pi # 1/2 rotation per second @@ -41,10 +40,10 @@ class Drivetrain: """ def __init__(self) -> None: - self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) - self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) - self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) - self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.Translation2d(-0.381, -0.381) self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) @@ -55,16 +54,17 @@ class Drivetrain: wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField) self.gyro = wpilib.AnalogGyro(0) - self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro) + self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro) + self._yaw = 0.0 - self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.kinematics = wpimath.SwerveDrive4Kinematics( self.frontLeftLocation, self.frontRightLocation, self.backLeftLocation, self.backRightLocation, ) - self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.odometry = wpimath.SwerveDrive4Odometry( self.kinematics, self.gyro.getRotation2d(), ( @@ -75,7 +75,7 @@ class Drivetrain: ), ) - self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + self.targetChassisVelocities = wpimath.ChassisVelocities() self.gyro.reset() @@ -95,27 +95,24 @@ class Drivetrain: :param fieldRelative: Whether the provided x and y speeds are relative to the field. :param periodSeconds: Time """ - swerveModuleStates = self.kinematics.toSwerveModuleStates( - wpimath.kinematics.ChassisSpeeds.discretize( - ( - wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, self.gyro.getRotation2d() - ) - if fieldRelative - else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot) - ), - periodSeconds, + chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot) + if fieldRelative: + chassisVelocities = chassisVelocities.toRobotRelative( + self.gyro.getRotation2d() ) - ) - wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, kMaxSpeed - ) - self.frontLeft.setDesiredState(swerveModuleStates[0]) - self.frontRight.setDesiredState(swerveModuleStates[1]) - self.backLeft.setDesiredState(swerveModuleStates[2]) - self.backRight.setDesiredState(swerveModuleStates[3]) - self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates) + chassisVelocities = chassisVelocities.discretize(periodSeconds) + swerveModuleStates = self.kinematics.desaturateWheelVelocities( + self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed + ) + self.frontLeft.setDesiredVelocity(swerveModuleStates[0]) + self.frontRight.setDesiredVelocity(swerveModuleStates[1]) + self.backLeft.setDesiredVelocity(swerveModuleStates[2]) + self.backRight.setDesiredVelocity(swerveModuleStates[3]) + + self.targetChassisVelocities = self.kinematics.toChassisVelocities( + swerveModuleStates + ) def updateOdometry(self) -> None: """Updates the field relative position of the robot.""" @@ -129,26 +126,26 @@ class Drivetrain: ), ) - def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]: return [ - self.frontLeft.getState(), - self.frontRight.getState(), - self.backLeft.getState(), - self.backRight.getState(), + self.frontLeft.getVelocity(), + self.frontRight.getVelocity(), + self.backLeft.getVelocity(), + self.backRight.getVelocity(), ] - def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: + def getModulePoses(self) -> list[wpimath.Pose2d]: p = self.odometry.getPose() - flTrans = wpimath.geometry.Transform2d( + flTrans = wpimath.Transform2d( self.frontLeftLocation, self.frontLeft.getAbsoluteHeading() ) - frTrans = wpimath.geometry.Transform2d( + frTrans = wpimath.Transform2d( self.frontRightLocation, self.frontRight.getAbsoluteHeading() ) - blTrans = wpimath.geometry.Transform2d( + blTrans = wpimath.Transform2d( self.backLeftLocation, self.backLeft.getAbsoluteHeading() ) - brTrans = wpimath.geometry.Transform2d( + brTrans = wpimath.Transform2d( self.backRightLocation, self.backRight.getAbsoluteHeading() ) return [ @@ -158,8 +155,8 @@ class Drivetrain: p.transformBy(brTrans), ] - def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: - return self.kinematics.toChassisSpeeds(self.getModuleStates()) + def getChassisVelocities(self) -> wpimath.ChassisVelocities: + return self.kinematics.toChassisVelocities(self.getModuleVelocities()) def log(self): table = "Drive/" @@ -169,7 +166,7 @@ class Drivetrain: wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) - chassisSpeeds = self.getChassisSpeeds() + chassisSpeeds = self.getChassisVelocities() wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) wpilib.SmartDashboard.putNumber( @@ -177,13 +174,13 @@ class Drivetrain: ) wpilib.SmartDashboard.putNumber( - table + "Target VX", self.targetChassisSpeeds.vx + table + "Target VX", self.targetChassisVelocities.vx ) wpilib.SmartDashboard.putNumber( - table + "Target VY", self.targetChassisSpeeds.vy + table + "Target VY", self.targetChassisVelocities.vy ) wpilib.SmartDashboard.putNumber( - table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps + table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps ) self.frontLeft.log() @@ -199,5 +196,7 @@ class Drivetrain: self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() self.backRight.simulationPeriodic() - self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) - self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) + rate = -1.0 * self.getChassisVelocities().omega_dps + self.simGyro.setRate(rate) + self._yaw += rate * 0.02 + self.simGyro.setYaw(self._yaw) diff --git a/photonlib-python-examples/aimattarget/robot.py b/photonlib-python-examples/aimattarget/robot.py index 692ca7f16..659be471f 100644 --- a/photonlib-python-examples/aimattarget/robot.py +++ b/photonlib-python-examples/aimattarget/robot.py @@ -32,9 +32,11 @@ VISION_TURN_kP = 0.01 class MyRobot(wpilib.TimedRobot): - def robotInit(self) -> None: + def __init__(self) -> None: """Robot initialization function""" - self.controller = wpilib.XboxController(0) + super().__init__() + + self.controller = wpilib.NiDsXboxController(0) self.swerve = drivetrain.Drivetrain() self.cam = PhotonCamera("YOUR CAMERA NAME") diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py index d269c620c..526119c38 100644 --- a/photonlib-python-examples/aimattarget/swervemodule.py +++ b/photonlib-python-examples/aimattarget/swervemodule.py @@ -26,11 +26,7 @@ import math import wpilib import wpilib.simulation -import wpimath.controller -import wpimath.filter -import wpimath.geometry -import wpimath.kinematics -import wpimath.trajectory +import wpimath import wpimath.units kWheelRadius = 0.0508 @@ -60,7 +56,7 @@ class SwerveModule: :param turningEncoderChannelB: DIO input for the turning encoder channel B """ self.moduleNumber = moduleNumber - self.desiredState = wpimath.kinematics.SwerveModuleState() + self.desiredVelocity = wpimath.SwerveModuleVelocity() self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) @@ -71,14 +67,14 @@ class SwerveModule: ) # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) + self.drivePIDController = wpimath.PIDController(10, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = wpimath.controller.PIDController(30, 0, 0) + self.turningPIDController = wpimath.PIDController(30, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) - self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) + self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7) # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -99,66 +95,60 @@ class SwerveModule: # Simulation Support self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder) self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder) - self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor) - self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor) - self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.1, 0.02 - ) - self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.0001, 0.02 - ) + self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel) + self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel) + self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02) + self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02) self.simTurningEncoderPos = 0 self.simDrivingEncoderPos = 0 - def getState(self) -> wpimath.kinematics.SwerveModuleState: + def getVelocity(self) -> wpimath.SwerveModuleVelocity: """Returns the current state of the module. :returns: The current state of the module. """ - return wpimath.kinematics.SwerveModuleState( + return wpimath.SwerveModuleVelocity( self.driveEncoder.getRate(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + def getPosition(self) -> wpimath.SwerveModulePosition: """Returns the current position of the module. :returns: The current position of the module. """ - return wpimath.kinematics.SwerveModulePosition( + return wpimath.SwerveModulePosition( self.driveEncoder.getDistance(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def setDesiredState( - self, desiredState: wpimath.kinematics.SwerveModuleState - ) -> None: + def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None: """Sets the desired state for the module. - :param desiredState: Desired state with speed and angle. + :param desiredVelocity: Desired state with speed and angle. """ - self.desiredState = desiredState + self.desiredVelocity = desiredVelocity - encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance()) # Optimize the reference state to avoid spinning further than 90 degrees - desiredState.optimize(encoderRotation) + desiredVelocity.optimize(encoderRotation) # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired # direction of travel that can occur when modules change directions. This results in smoother # driving. - desiredState.speed *= (desiredState.angle - encoderRotation).cos() + desiredVelocity.velocity *= (desiredVelocity.angle - encoderRotation).cos() # Calculate the drive output from the drive PID controller. driveOutput = self.drivePIDController.calculate( - self.driveEncoder.getRate(), desiredState.speed + self.driveEncoder.getRate(), desiredVelocity.velocity ) - driveFeedforward = self.driveFeedforward.calculate(desiredState.speed) + driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity) # Calculate the turning motor output from the turning PID controller. turnOutput = self.turningPIDController.calculate( - self.turningEncoder.getDistance(), desiredState.angle.radians() + self.turningEncoder.getDistance(), desiredVelocity.angle.radians() ) turnFeedforward = self.turnFeedforward.calculate( @@ -168,11 +158,11 @@ class SwerveModule: self.driveMotor.setVoltage(driveOutput + driveFeedforward) self.turningMotor.setVoltage(turnOutput + turnFeedforward) - def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: - return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + def getAbsoluteHeading(self) -> wpimath.Rotation2d: + return wpimath.Rotation2d(self.turningEncoder.getDistance()) def log(self) -> None: - state = self.getState() + state = self.getVelocity() table = "Module " + str(self.moduleNumber) + "/" wpilib.SmartDashboard.putNumber( @@ -183,15 +173,17 @@ class SwerveModule: table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()), ) - wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps) wpilib.SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", self.desiredState.speed_fps + table + "Drive Velocity Feet", state.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Drive Voltage", self.driveMotor.get() * 12.0 + table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Steer Voltage", self.turningMotor.get() * 12.0 + table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0 + ) + wpilib.SmartDashboard.putNumber( + table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0 ) def simulationPeriodic(self) -> None: diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index 0e892e5b5..bf295b40c 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -28,16 +28,14 @@ import math import swervemodule import wpilib import wpilib.simulation -import wpimath.estimator -import wpimath.geometry -import wpimath.kinematics +import wpimath kMaxSpeed = 3.0 # 3 meters per second kMaxAngularSpeed = math.pi # 1/2 rotation per second -kInitialPose = wpimath.geometry.Pose2d( - wpimath.geometry.Translation2d(1.0, 1.0), - wpimath.geometry.Rotation2d.fromDegrees(0.0), +kInitialPose = wpimath.Pose2d( + wpimath.Translation2d(1.0, 1.0), + wpimath.Rotation2d.fromDegrees(0.0), ) @@ -47,10 +45,10 @@ class Drivetrain: """ def __init__(self) -> None: - self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) - self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) - self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) - self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.Translation2d(-0.381, -0.381) self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) @@ -61,16 +59,17 @@ class Drivetrain: wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField) self.gyro = wpilib.AnalogGyro(0) - self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro) + self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro) + self._yaw = 0.0 - self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.kinematics = wpimath.SwerveDrive4Kinematics( self.frontLeftLocation, self.frontRightLocation, self.backLeftLocation, self.backRightLocation, ) - self.poseEst = wpimath.estimator.SwerveDrive4PoseEstimator( + self.poseEst = wpimath.SwerveDrive4PoseEstimator( self.kinematics, self.gyro.getRotation2d(), ( @@ -79,10 +78,9 @@ class Drivetrain: self.backLeft.getPosition(), self.backRight.getPosition(), ), - kInitialPose, ) - self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + self.targetChassisVelocities = wpimath.ChassisVelocities() self.gyro.reset() @@ -102,31 +100,28 @@ class Drivetrain: :param fieldRelative: Whether the provided x and y speeds are relative to the field. :param periodSeconds: Time """ - swerveModuleStates = self.kinematics.toSwerveModuleStates( - wpimath.kinematics.ChassisSpeeds.discretize( - ( - wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, self.gyro.getRotation2d() - ) - if fieldRelative - else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot) - ), - periodSeconds, + chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot) + if fieldRelative: + chassisVelocities = chassisVelocities.toRobotRelative( + self.gyro.getRotation2d() ) - ) - wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, kMaxSpeed - ) - self.frontLeft.setDesiredState(swerveModuleStates[0]) - self.frontRight.setDesiredState(swerveModuleStates[1]) - self.backLeft.setDesiredState(swerveModuleStates[2]) - self.backRight.setDesiredState(swerveModuleStates[3]) - self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates) + chassisVelocities = chassisVelocities.discretize(periodSeconds) + swerveModuleStates = self.kinematics.desaturateWheelVelocities( + self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed + ) + self.frontLeft.setDesiredVelocity(swerveModuleStates[0]) + self.frontRight.setDesiredVelocity(swerveModuleStates[1]) + self.backLeft.setDesiredVelocity(swerveModuleStates[2]) + self.backRight.setDesiredVelocity(swerveModuleStates[3]) + + self.targetChassisVelocities = self.kinematics.toChassisVelocities( + swerveModuleStates + ) def updateOdometry(self) -> None: """Updates the field relative position of the robot.""" - self.poseEst.update( + self.odometry.update( self.gyro.getRotation2d(), ( self.frontLeft.getPosition(), @@ -136,9 +131,7 @@ class Drivetrain: ), ) - def addVisionPoseEstimate( - self, pose: wpimath.geometry.Pose3d, timestamp: float - ) -> None: + def addVisionPoseEstimate(self, pose: wpimath.Pose3d, timestamp: float) -> None: self.poseEst.addVisionMeasurement(pose, timestamp) def resetPose(self) -> None: @@ -153,26 +146,26 @@ class Drivetrain: kInitialPose, ) - def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]: return [ - self.frontLeft.getState(), - self.frontRight.getState(), - self.backLeft.getState(), - self.backRight.getState(), + self.frontLeft.getVelocity(), + self.frontRight.getVelocity(), + self.backLeft.getVelocity(), + self.backRight.getVelocity(), ] - def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: + def getModulePoses(self) -> list[wpimath.Pose2d]: p = self.poseEst.getEstimatedPosition() - flTrans = wpimath.geometry.Transform2d( + flTrans = wpimath.Transform2d( self.frontLeftLocation, self.frontLeft.getAbsoluteHeading() ) - frTrans = wpimath.geometry.Transform2d( + frTrans = wpimath.Transform2d( self.frontRightLocation, self.frontRight.getAbsoluteHeading() ) - blTrans = wpimath.geometry.Transform2d( + blTrans = wpimath.Transform2d( self.backLeftLocation, self.backLeft.getAbsoluteHeading() ) - brTrans = wpimath.geometry.Transform2d( + brTrans = wpimath.Transform2d( self.backRightLocation, self.backRight.getAbsoluteHeading() ) return [ @@ -182,18 +175,18 @@ class Drivetrain: p.transformBy(brTrans), ] - def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: - return self.kinematics.toChassisSpeeds(self.getModuleStates()) + def getChassisVelocities(self) -> wpimath.ChassisVelocities: + return self.kinematics.toChassisVelocities(self.getModuleVelocities()) def log(self): table = "Drive/" - pose = self.poseEst.getEstimatedPosition() + pose = self.odometry.getPose() wpilib.SmartDashboard.putNumber(table + "X", pose.X()) wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) - chassisSpeeds = self.getChassisSpeeds() + chassisSpeeds = self.getChassisVelocities() wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) wpilib.SmartDashboard.putNumber( @@ -201,13 +194,13 @@ class Drivetrain: ) wpilib.SmartDashboard.putNumber( - table + "Target VX", self.targetChassisSpeeds.vx + table + "Target VX", self.targetChassisVelocities.vx ) wpilib.SmartDashboard.putNumber( - table + "Target VY", self.targetChassisSpeeds.vy + table + "Target VY", self.targetChassisVelocities.vy ) wpilib.SmartDashboard.putNumber( - table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps + table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps ) self.frontLeft.log() @@ -215,7 +208,7 @@ class Drivetrain: self.backLeft.log() self.backRight.log() - self.debugField.getRobotObject().setPose(self.poseEst.getEstimatedPosition()) + self.debugField.getRobotObject().setPose(self.odometry.getPose()) self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) def simulationPeriodic(self): @@ -223,5 +216,7 @@ class Drivetrain: self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() self.backRight.simulationPeriodic() - self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) - self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) + rate = -1.0 * self.getChassisVelocities().omega_dps + self.simGyro.setRate(rate) + self._yaw += rate * 0.02 + self.simGyro.setYaw(self._yaw) diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py index f06e98286..edf63bda5 100644 --- a/photonlib-python-examples/poseest/robot.py +++ b/photonlib-python-examples/poseest/robot.py @@ -26,20 +26,22 @@ import drivetrain import wpilib -import wpimath.geometry +import wpimath from photonlibpy import PhotonCamera, PhotonPoseEstimator from robotpy_apriltag import AprilTagField, AprilTagFieldLayout -kRobotToCam = wpimath.geometry.Transform3d( - wpimath.geometry.Translation3d(0.5, 0.0, 0.5), - wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0), +kRobotToCam = wpimath.Transform3d( + wpimath.Translation3d(0.5, 0.0, 0.5), + wpimath.Rotation3d.fromDegrees(0.0, -30.0, 0.0), ) class MyRobot(wpilib.TimedRobot): - def robotInit(self) -> None: + def __init__(self) -> None: """Robot initialization function""" - self.controller = wpilib.XboxController(0) + super().__init__() + + self.controller = wpilib.NiDsXboxController(0) self.swerve = drivetrain.Drivetrain() self.cam = PhotonCamera("YOUR CAMERA NAME") self.camPoseEst = PhotonPoseEstimator( diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index 59cf9e9c3..058ef5115 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -1,4 +1,4 @@ -################################################################################### +Speeds ################################################################################### # MIT License # # Copyright (c) PhotonVision @@ -26,11 +26,7 @@ import math import wpilib import wpilib.simulation -import wpimath.controller -import wpimath.filter -import wpimath.geometry -import wpimath.kinematics -import wpimath.trajectory +import wpimath import wpimath.units kWheelRadius = 0.0508 @@ -60,7 +56,7 @@ class SwerveModule: :param turningEncoderChannelB: DIO input for the turning encoder channel B """ self.moduleNumber = moduleNumber - self.desiredState = wpimath.kinematics.SwerveModuleState() + self.desiredVelocity = wpimath.SwerveModuleVelocity() self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) @@ -71,13 +67,14 @@ class SwerveModule: ) # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) + self.drivePIDController = wpimath.PIDController(10, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = wpimath.controller.PIDController(30, 0, 0) + self.turningPIDController = wpimath.PIDController(30, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) + self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7) # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -98,76 +95,74 @@ class SwerveModule: # Simulation Support self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder) self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder) - self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor) - self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor) - self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.1, 0.02 - ) - self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR( - 0.0001, 0.02 - ) + self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel) + self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel) + self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02) + self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02) self.simTurningEncoderPos = 0 self.simDrivingEncoderPos = 0 - def getState(self) -> wpimath.kinematics.SwerveModuleState: + def getVelocity(self) -> wpimath.SwerveModuleVelocity: """Returns the current state of the module. :returns: The current state of the module. """ - return wpimath.kinematics.SwerveModuleState( + return wpimath.SwerveModuleVelocity( self.driveEncoder.getRate(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + def getPosition(self) -> wpimath.SwerveModulePosition: """Returns the current position of the module. :returns: The current position of the module. """ - return wpimath.kinematics.SwerveModulePosition( + return wpimath.SwerveModulePosition( self.driveEncoder.getDistance(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + wpimath.Rotation2d(self.turningEncoder.getDistance()), ) - def setDesiredState( - self, desiredState: wpimath.kinematics.SwerveModuleState - ) -> None: + def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None: """Sets the desired state for the module. - :param desiredState: Desired state with speed and angle. + :param desiredVelocity: Desired state with speed and angle. """ - self.desiredState = desiredState + self.desiredVelocity = desiredVelocity - encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance()) # Optimize the reference state to avoid spinning further than 90 degrees - self.desiredState.optimize(encoderRotation) + self.desiredVelocity.optimize(encoderRotation) # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired # direction of travel that can occur when modules change directions. This results in smoother # driving. - self.desiredState.speed *= (self.desiredState.angle - encoderRotation).cos() + self.desiredVelocity.velocity *= ( + self.desiredVelocity.angle - encoderRotation + ).cos() # Calculate the drive output from the drive PID controller. driveOutput = self.drivePIDController.calculate( - self.driveEncoder.getRate(), self.desiredState.speed + self.driveEncoder.getRate(), self.desiredVelocity.velocity ) - driveFeedforward = self.driveFeedforward.calculate(self.desiredState.speed) + driveFeedforward = self.driveFeedforward.calculate( + self.desiredVelocity.velocity + ) # Calculate the turning motor output from the turning PID controller. turnOutput = self.turningPIDController.calculate( - self.turningEncoder.getDistance(), self.desiredState.angle.radians() + self.turningEncoder.getDistance(), self.desiredVelocity.angle.radians() ) self.driveMotor.setVoltage(driveOutput + driveFeedforward) - self.turningMotor.setVoltage(turnOutput) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) - def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: - return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + def getAbsoluteHeading(self) -> wpimath.Rotation2d: + return wpimath.Rotation2d(self.turningEncoder.getDistance()) def log(self) -> None: - state = self.getState() + state = self.getVelocity() table = "Module " + str(self.moduleNumber) + "/" wpilib.SmartDashboard.putNumber( @@ -178,15 +173,17 @@ class SwerveModule: table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()), ) - wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps) wpilib.SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", self.desiredState.speed_fps + table + "Drive Velocity Feet", state.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Drive Voltage", self.driveMotor.get() * 12.0 + table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps ) wpilib.SmartDashboard.putNumber( - table + "Steer Voltage", self.turningMotor.get() * 12.0 + table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0 + ) + wpilib.SmartDashboard.putNumber( + table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0 ) def simulationPeriodic(self) -> None: @@ -199,7 +196,9 @@ class SwerveModule: driveSpdRaw = ( driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0) ) - turnSpdRaw = turnVoltage / 0.7 + turnSpdRaw = ( + turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0) + ) driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw) turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw) self.simDrivingEncoderPos += 0.02 * driveSpd diff --git a/photonlib-python-examples/run.bat b/photonlib-python-examples/run.bat index dcb872588..ea61b8bd1 100644 --- a/photonlib-python-examples/run.bat +++ b/photonlib-python-examples/run.bat @@ -22,4 +22,5 @@ set PYTHONPATH=%PHOTONLIBPY_ROOT% cd %~1 :: Run the example +mypy --show-column-numbers --config-file ../../photon-lib/py/pyproject.toml . robotpy sim diff --git a/photonlib-python-examples/run.sh b/photonlib-python-examples/run.sh index e6838b135..301dc3f9a 100755 --- a/photonlib-python-examples/run.sh +++ b/photonlib-python-examples/run.sh @@ -1,3 +1,4 @@ +set -e # Check if the first argument is provided if [ $# -eq 0 ] then @@ -9,4 +10,5 @@ fi cd $1 # Run the example +mypy --show-column-numbers --config-file ../../photon-lib/py/pyproject.toml . python3 robot.py sim diff --git a/shared/PhotonVersion.cpp.in b/shared/PhotonVersion.cpp.in index 438ee019e..1cfbf8787 100644 --- a/shared/PhotonVersion.cpp.in +++ b/shared/PhotonVersion.cpp.in @@ -32,6 +32,5 @@ namespace photon { // Versions of dependant libraries const char* wpilibTargetVersion = "${wpilibVersion}"; - const char* opencvTargetVersion = "${opencvVersion}"; } } diff --git a/shared/common.gradle b/shared/common.gradle index 5b97514c5..0db1f2d2c 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -32,10 +32,8 @@ 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, openCVversion) + implementation wpilibTools.deps.wpilibOpenCvJava(openCVversion) - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion implementation group: "io.avaje", name: "avaje-jsonb-jackson", version: avajeJsonbVersion diff --git a/shared/config.gradle b/shared/config.gradle index 7c8b3066e..977252691 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -6,10 +6,7 @@ nativeUtils.withCrossSystemCore() // Configure WPI dependencies. nativeUtils.wpi.configureDependencies { wpiVersion = wpilibVersion - wpimathVersion = wpimathVersion - opencvYear = 'frc'+openCVYear opencvVersion = openCVversion - niLibVersion = "2026.1.0" } // Configure warnings and errors diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index b4f7481c8..2ac23cd1a 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -148,10 +148,8 @@ 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, openCVversion) + implementation wpilibTools.deps.wpilibOpenCvJava(openCVversion) - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion diff --git a/versioningHelper.gradle b/versioningHelper.gradle deleted file mode 100644 index 4a91834c1..000000000 --- a/versioningHelper.gradle +++ /dev/null @@ -1,42 +0,0 @@ -import java.nio.file.Path -import java.time.LocalDateTime -import java.time.format.DateTimeFormatter - -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" - } - - // 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 -} - -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) -}