diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index d5d59cc6f1..5cdc15f846 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -18,7 +18,7 @@ Steps to reproduce the behavior: 4. See error **Screenshots / Videos** -If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. +If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. If your issue is regarding the web dashboard, please provide screenshots and the output of the browser console. **Platform:** - Hardware Platform (ex. Raspberry Pi 4, Windows x64): diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7a0365769c..58d2356305 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -16,16 +16,16 @@ jobs: working-directory: photon-client runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Node.js - uses: actions/setup-node@v3 + uses: actions/setup-node@v4 with: node-version: 18 - name: Install Dependencies run: npm ci - name: Build Production Client run: npm run build - - uses: actions/upload-artifact@master + - uses: actions/upload-artifact@v4 with: name: built-client path: photon-client/dist/ @@ -34,13 +34,13 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - name: Fetch tags run: git fetch --tags --force - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -68,7 +68,7 @@ jobs: steps: # Checkout code. - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - name: Fetch tags @@ -98,11 +98,11 @@ jobs: name: "Build Offline Docs" runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: repository: 'PhotonVision/photonvision-docs.git' ref: master - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: '3.9' - name: Install dependencies @@ -136,11 +136,11 @@ jobs: name: "Photonlib - Build Host - ${{ matrix.artifact-name }}" runs-on: ${{ matrix.os }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -169,7 +169,7 @@ jobs: container: ${{ matrix.container }} name: "Photonlib - Build Docker - ${{ matrix.artifact-name }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Config Git @@ -222,11 +222,11 @@ jobs: name: "Build fat JAR - ${{ matrix.artifact-name }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Java 17 - uses: actions/setup-java@v3 + uses: actions/setup-java@v4 with: java-version: 17 distribution: temurin @@ -238,11 +238,11 @@ jobs: del photon-server\src\main\resources\web\*.* mkdir photon-server\src\main\resources\web\docs if: ${{ (matrix.os) == 'windows-latest' }} - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: built-client path: photon-server/src/main/resources/web/ - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: built-docs path: photon-server/src/main/resources/web/docs @@ -254,7 +254,7 @@ jobs: chmod +x gradlew ./gradlew photon-server:shadowJar --max-workers 2 if: ${{ (matrix.arch-override == 'none') }} - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 with: name: jar-${{ matrix.artifact-name }} path: photon-server/build/libs @@ -280,17 +280,17 @@ jobs: steps: - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v4 with: name: jar-${{ matrix.artifact-name }} - name: Generate image run: | chmod +x scripts/generatePiImage.sh ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }} - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 name: Upload image with: name: image-${{ matrix.image_suffix }} @@ -301,7 +301,7 @@ jobs: steps: # Download literally every single artifact. This also downloads client and docs, # but the filtering below won't pick these up (I hope) - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v4 - run: find # Push to dev release - uses: pyTooling/Actions/releaser@r0 diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml new file mode 100644 index 0000000000..de54a54756 --- /dev/null +++ b/.github/workflows/python.yml @@ -0,0 +1,62 @@ +name: Build and Distribute PhotonLibPy + +permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing + +on: + push: + branches: [ master ] + tags: + - 'v*' + pull_request: + branches: [ master ] + +jobs: + buildAndDeploy: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + sparse-checkout-cone-mode: false + fetch-tags: true + fetch-depth: 99999 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: 3.11 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install setuptools wheel pytest + + - name: Build wheel + working-directory: ./photon-lib/py + run: | + python setup.py sdist bdist_wheel + + - name: Run Unit Tests + working-directory: ./photon-lib/py + run: | + pip install --no-cache-dir dist/*.whl + pytest + + + - name: Upload artifacts + uses: actions/upload-artifact@master + with: + name: dist + path: ./photon-lib/py/dist/ + + - name: Publish package distributions to TestPyPI + # Only upload on tags + if: startsWith(github.ref, 'refs/tags/v') + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages_dir: ./photon-lib/py/dist/ + + permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing diff --git a/build.gradle b/build.gradle index c0792d03f2..3c00130fd5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false + id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" id 'edu.wpi.first.WpilibTools' version '1.3.0' } @@ -20,7 +20,8 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3" + wpilibVersion = "2024.1.1-beta-4" + wpimathVersion = wpilibVersion openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" @@ -36,16 +37,11 @@ ext { if (nativeName == "macx64") nativeName = "osxx86-64"; if (nativeName == "macarm64") nativeName = "osxarm64"; jniPlatform = nativeName - println("Building for platform " + jniPlatform) + println("Building for platform: " + jniPlatform) + println("Using Wpilib: " + wpilibVersion) + println("Using OpenCV: " + openCVversion) } -wpilibTools.deps.wpilibVersion = wpilibVersion - -// Tell gradlerio what version of things to use (that we care about) -// See: https://github.com/wpilibsuite/GradleRIO/blob/main/src/main/java/edu/wpi/first/gradlerio/wpi/WPIVersionsExtension.java -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); - spotless { java { target fileTree('.') { diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 13f3e75776..a836e4b362 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -8,7 +8,7 @@ import PvIcon from "@/components/common/pv-icon.vue"; const props = defineProps<{ streamType: "Raw" | "Processed"; - id?: string; + id: string; }>(); const streamSrc = computed(() => { @@ -25,8 +25,6 @@ const streamDesc = computed(() => `${props.streamType} Stream View`); const streamStyle = computed(() => { if (useStateStore().colorPickingMode) { return { width: "100%", cursor: "crosshair" }; - } else if (streamSrc.value !== loadingImage) { - return { width: "100%", cursor: "pointer" }; } return { width: "100%" }; @@ -40,11 +38,6 @@ const overlayStyle = computed(() => { } }); -const handleStreamClick = () => { - if (!useStateStore().colorPickingMode && streamSrc.value !== loadingImage) { - window.open(streamSrc.value); - } -}; const handleCaptureClick = () => { if (props.streamType === "Raw") { useCameraSettingsStore().saveInputSnapshot(); @@ -52,18 +45,19 @@ const handleCaptureClick = () => { useCameraSettingsStore().saveOutputSnapshot(); } }; +const handlePopoutClick = () => { + window.open(streamSrc.value); +}; +const handleFullscreenRequest = () => { + const stream = document.getElementById(props.id); + if (!stream) return; + stream.requestFullscreen(); +}; @@ -81,6 +87,7 @@ const handleCaptureClick = () => { } .stream-overlay { + display: flex; opacity: 0; transition: 0.1s ease; position: absolute; diff --git a/photon-client/src/components/cameras/CamerasView.vue b/photon-client/src/components/cameras/CamerasView.vue index f7c110cf62..1d52bfcf05 100644 --- a/photon-client/src/components/cameras/CamerasView.vue +++ b/photon-client/src/components/cameras/CamerasView.vue @@ -78,10 +78,20 @@ const fpsTooLow = computed(() => {
- +
- +
diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue index 2375523b96..4de10823a9 100644 --- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue +++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue @@ -50,7 +50,7 @@ const resetCurrentBuffer = () => { useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - Ambiguity % + Ambiguity Ratio @@ -82,7 +82,7 @@ const resetCurrentBuffer = () => { useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) + "%" : "(In Multi-Target)" }} + {{ target.ambiguity >= 0 ? target.ambiguity.toFixed(2) : "(In Multi-Target)" }} @@ -116,9 +116,18 @@ const resetCurrentBuffer = () => { {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }} m {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }} m {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(2) }} m - {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x.toFixed(2) }}° - {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y.toFixed(2) }}° - {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z.toFixed(2) }}° + {{ ( + useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x * + (180.0 / Math.PI) + ).toFixed(2) }}° + {{ ( + useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y * + (180.0 / Math.PI) + ).toFixed(2) }}° + {{ ( + useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z * + (180.0 / Math.PI) + ).toFixed(2) }}° {{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }} diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 6fbf407797..d5de014915 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -1,38 +1,24 @@ -plugins { - id 'edu.wpi.first.WpilibTools' version '1.3.0' -} - import java.nio.file.Path apply from: "${rootDir}/shared/common.gradle" dependencies { - implementation project(':photon-targeting') - - implementation "io.javalin:javalin:$javalinVersion" - - implementation 'org.msgpack:msgpack-core:0.9.0' - implementation 'org.msgpack:jackson-dataformat-msgpack:0.9.0' - // JOGL stuff (currently we only distribute for aarch64, which is Pi 4) implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion" implementation "org.jogamp.jogl:jogl-all:$joglVersion" - implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-linux-aarch64" implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-linux-aarch64" // Zip implementation 'org.zeroturnaround:zt-zip:1.14' - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation "org.xerial:sqlite-jdbc:3.41.0.0" } -task writeCurrentVersionJava { +task writeCurrentVersion { def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in") writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"), versionString) } -build.dependsOn writeCurrentVersionJava +build.dependsOn writeCurrentVersion diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java index 1046c3d6c6..e162f6aa16 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java @@ -62,12 +62,13 @@ enum ConfigSaveStrategy { public static ConfigManager getInstance() { if (INSTANCE == null) { + Path rootFolder = PathManager.getInstance().getRootFolder(); switch (m_saveStrat) { case SQL: - INSTANCE = new ConfigManager(getRootFolder(), new SqlConfigProvider(getRootFolder())); + INSTANCE = new ConfigManager(rootFolder, new SqlConfigProvider(rootFolder)); break; case LEGACY: - INSTANCE = new ConfigManager(getRootFolder(), new LegacyConfigProvider(getRootFolder())); + INSTANCE = new ConfigManager(rootFolder, new LegacyConfigProvider(rootFolder)); break; case ATOMIC_ZIP: // not yet done, fall through @@ -78,7 +79,7 @@ public static ConfigManager getInstance() { return INSTANCE; } - private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General); + private static final Logger logger = new Logger(ConfigManager.class, LogGroup.Config); private void translateLegacyIfPresent(Path folderPath) { if (!(m_provider instanceof SqlConfigProvider)) { @@ -167,7 +168,7 @@ public PhotonConfiguration getConfig() { } private static Path getRootFolder() { - return Path.of("photonvision_config"); + return PathManager.getInstance().getRootFolder(); } ConfigManager(Path configDirectory, ConfigProvider provider) { diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java new file mode 100644 index 0000000000..0a57d13d03 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PathManager.java @@ -0,0 +1,75 @@ +/* + * 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.configuration; + +import java.io.File; +import java.nio.file.Path; +import java.text.DateFormat; +import java.text.ParseException; +import java.text.SimpleDateFormat; +import java.time.LocalDateTime; +import java.time.format.DateTimeFormatter; +import java.time.temporal.TemporalAccessor; +import java.util.Date; + +public class PathManager { + private static PathManager INSTANCE; + + final File configDirectoryFile; + + public static PathManager getInstance() { + if (INSTANCE == null) { + INSTANCE = new PathManager(); + } + return INSTANCE; + } + + private PathManager() { + this.configDirectoryFile = new File(getRootFolder().toUri()); + } + + public Path getRootFolder() { + return Path.of("photonvision_config"); + } + + public Path getLogsDir() { + return Path.of(configDirectoryFile.toString(), "logs"); + } + + public static final String LOG_PREFIX = "photonvision-"; + public static final String LOG_EXT = ".log"; + public static final String LOG_DATE_TIME_FORMAT = "yyyy-M-d_hh-mm-ss"; + + public String taToLogFname(TemporalAccessor date) { + var dateString = DateTimeFormatter.ofPattern(LOG_DATE_TIME_FORMAT).format(date); + return LOG_PREFIX + dateString + LOG_EXT; + } + + public Path getLogPath() { + var logFile = Path.of(this.getLogsDir().toString(), taToLogFname(LocalDateTime.now())).toFile(); + if (!logFile.getParentFile().exists()) logFile.getParentFile().mkdirs(); + return logFile.toPath(); + } + + public Date logFnameToDate(String fname) throws ParseException { + // Strip away known unneeded portions of the log file name + fname = fname.replace(LOG_PREFIX, "").replace(LOG_EXT, ""); + DateFormat format = new SimpleDateFormat(LOG_DATE_TIME_FORMAT); + return format.parse(fname); + } +} diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 8a0c755ad8..883b3607d8 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -45,12 +45,13 @@ *

Global has one row per global config file (like hardware settings and network settings) */ public class SqlConfigProvider extends ConfigProvider { - private final Logger logger = new Logger(SqlConfigProvider.class, LogGroup.Config); + private static final Logger logger = new Logger(SqlConfigProvider.class, LogGroup.Config); static class TableKeys { static final String CAM_UNIQUE_NAME = "unique_name"; static final String CONFIG_JSON = "config_json"; static final String DRIVERMODE_JSON = "drivermode_json"; + static final String OTHERPATHS_JSON = "otherpaths_json"; static final String PIPELINE_JSONS = "pipeline_jsons"; static final String NETWORK_CONFIG = "networkConfig"; @@ -147,6 +148,7 @@ private void initDatabase() { + " unique_name TINYTEXT PRIMARY KEY,\n" + " config_json text NOT NULL,\n" + " drivermode_json text NOT NULL,\n" + + " otherpaths_json text NOT NULL,\n" + " pipeline_jsons mediumtext NOT NULL\n" + ");"; createCameraTableStatement.execute(sql); @@ -295,8 +297,8 @@ private void saveCameras(Connection conn) { try { // Replace this camera's row with the new settings var sqlString = - "REPLACE INTO cameras (unique_name, config_json, drivermode_json, pipeline_jsons) VALUES " - + "(?,?,?,?);"; + "REPLACE INTO cameras (unique_name, config_json, drivermode_json, otherpaths_json, pipeline_jsons) VALUES " + + "(?,?,?,?,?);"; for (var c : config.getCameraConfigurations().entrySet()) { PreparedStatement statement = conn.prepareStatement(sqlString); @@ -305,6 +307,7 @@ private void saveCameras(Connection conn) { statement.setString(1, c.getKey()); statement.setString(2, JacksonUtils.serializeToString(config)); statement.setString(3, JacksonUtils.serializeToString(config.driveModeSettings)); + statement.setString(4, JacksonUtils.serializeToString(config.otherPaths)); // Serializing a list of abstract classes sucks. Instead, make it into an array // of strings, which we can later unpack back into individual settings @@ -321,7 +324,7 @@ private void saveCameras(Connection conn) { }) .filter(Objects::nonNull) .collect(Collectors.toList()); - statement.setString(4, JacksonUtils.serializeToString(settings)); + statement.setString(5, JacksonUtils.serializeToString(settings)); statement.executeUpdate(); } @@ -455,10 +458,11 @@ private HashMap loadCameraConfigs(Connection conn) query = conn.prepareStatement( String.format( - "SELECT %s, %s, %s, %s FROM cameras", + "SELECT %s, %s, %s, %s, %s FROM cameras", TableKeys.CAM_UNIQUE_NAME, TableKeys.CONFIG_JSON, TableKeys.DRIVERMODE_JSON, + TableKeys.OTHERPATHS_JSON, TableKeys.PIPELINE_JSONS)); var result = query.executeQuery(); @@ -474,6 +478,8 @@ private HashMap loadCameraConfigs(Connection conn) var driverMode = JacksonUtils.deserialize( result.getString(TableKeys.DRIVERMODE_JSON), DriverModePipelineSettings.class); + var otherPaths = + JacksonUtils.deserialize(result.getString(TableKeys.OTHERPATHS_JSON), String[].class); List pipelineSettings = JacksonUtils.deserialize( result.getString(TableKeys.PIPELINE_JSONS), dummyList.getClass()); @@ -487,6 +493,7 @@ private HashMap loadCameraConfigs(Connection conn) config.pipelineSettings = loadedSettings; config.driveModeSettings = driverMode; + config.otherPaths = otherPaths; loadedConfigurations.put(uniqueName, config); } } catch (SQLException | IOException e) { diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/PiVersion.java b/photon-core/src/main/java/org/photonvision/common/hardware/PiVersion.java index dbf23ac7e4..86e63512fb 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/PiVersion.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/PiVersion.java @@ -27,6 +27,7 @@ public enum PiVersion { ZERO_2_W("Raspberry Pi Zero 2"), PI_3("Pi 3"), PI_4("Pi 4"), + PI_5("Pi 5"), COMPUTE_MODULE_3("Compute Module 3"), UNKNOWN("UNKNOWN"); diff --git a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java index b06fea13fa..0e2ef96ef5 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java @@ -24,7 +24,8 @@ import java.util.*; import java.util.function.Supplier; import org.apache.commons.lang3.tuple.Pair; -import org.photonvision.common.configuration.ConfigManager; +// import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.configuration.PathManager; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.util.TimedTaskManager; @@ -103,8 +104,8 @@ public static String format( static { currentAppenders.add(new ConsoleLogAppender()); currentAppenders.add(uiLogAppender); - addFileAppender(ConfigManager.getInstance().getLogPath()); - cleanLogs(ConfigManager.getInstance().getLogsDir()); + addFileAppender(PathManager.getInstance().getLogPath()); + cleanLogs(PathManager.getInstance().getLogsDir()); } @SuppressWarnings("ResultOfMethodCallIgnored") @@ -133,8 +134,7 @@ public static void cleanLogs(Path folderToClean) { logFileList.removeIf( (File arg0) -> { try { - logFileStartDateMap.put( - arg0, ConfigManager.getInstance().logFnameToDate(arg0.getName())); + logFileStartDateMap.put(arg0, PathManager.getInstance().logFnameToDate(arg0.getName())); return false; } catch (ParseException e) { return true; diff --git a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java index a75ae2e579..8a12aeabfa 100644 --- a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java +++ b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java @@ -64,6 +64,7 @@ public enum SensorModel { Disconnected, OV5647, // Picam v1 IMX219, // Picam v2 + IMX708, // Picam v3 IMX477, // Picam HQ OV9281, OV7251, @@ -77,6 +78,8 @@ public String getFriendlyName() { return "Camera Module v1"; case IMX219: return "Camera Module v2"; + case IMX708: + return "Camera Module v3"; case IMX477: return "HQ Camera"; case OV9281: diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 7e0bd5c5c7..2fdde391d2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -86,6 +86,9 @@ public LibcameraGpuSettables(CameraConfiguration configuration) { if (sensorModel == LibCameraJNI.SensorModel.IMX477) { LibcameraGpuSource.logger.warn( "It appears you are using a Pi HQ Camera. This camera is not officially supported. You will have to set your camera FOV differently based on resolution."); + } else if (sensorModel == LibCameraJNI.SensorModel.IMX708) { + LibcameraGpuSource.logger.warn( + "It appears you are using a Pi Camera V3. This camera is not officially supported. You will have to set your camera FOV differently based on resolution."); } else if (sensorModel == LibCameraJNI.SensorModel.Unknown) { LibcameraGpuSource.logger.warn( "You have an unknown sensor connected to your Pi over CSI! This is likely a bug. If it is not, then you will have to set your camera FOV differently based on resolution."); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 53d89167ec..05737ed293 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -17,6 +17,8 @@ package org.photonvision.vision.camera; +import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Objects; @@ -108,8 +110,20 @@ public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseNa for (var qc : quirkyCameras) { boolean hasBaseName = !qc.baseName.isEmpty(); boolean matchesBaseName = qc.baseName.equals(baseName) || !hasBaseName; + // If we have a quirkycamera we need to copy the quirks from our predefined object and create + // a quirkycamera object with the baseName. if (qc.usbVid == usbVid && qc.usbPid == usbPid && matchesBaseName) { - return qc; + List quirks = new ArrayList(); + for (var q : CameraQuirk.values()) { + if (qc.hasQuirk(q)) quirks.add(q); + } + QuirkyCamera c = + new QuirkyCamera( + usbVid, + usbPid, + baseName, + Arrays.copyOf(quirks.toArray(), quirks.size(), CameraQuirk[].class)); + return c; } } return new QuirkyCamera(usbVid, usbPid, baseName); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index 6717f4edd8..a2f2911431 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -28,7 +28,9 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.FrameProvider; +import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; @@ -37,10 +39,10 @@ public class USBCameraSource extends VisionSource { private final Logger logger; private final UsbCamera camera; private final USBCameraSettables usbCameraSettables; - private final USBFrameProvider usbFrameProvider; + private FrameProvider usbFrameProvider; private final CvSink cvSink; - public final QuirkyCamera cameraQuirks; + private QuirkyCamera cameraQuirks; public USBCameraSource(CameraConfiguration config) { super(config); @@ -77,6 +79,22 @@ public USBCameraSource(CameraConfiguration config) { } } + /** + * Mostly just used for unit tests to better simulate a usb camera without a camera being present. + */ + public USBCameraSource(CameraConfiguration config, int pid, int vid, boolean unitTest) { + this(config); + + cameraQuirks = QuirkyCamera.getQuirkyCamera(pid, vid, config.baseName); + + if (unitTest) + usbFrameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath( + TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + } + void disableAutoFocus() { if (cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) { try { @@ -88,6 +106,10 @@ void disableAutoFocus() { } } + public QuirkyCamera getCameraQuirks() { + return this.cameraQuirks; + } + @Override public FrameProvider getFrameProvider() { return usbFrameProvider; @@ -103,7 +125,7 @@ protected USBCameraSettables(CameraConfiguration configuration) { super(configuration); getAllVideoModes(); if (!cameraQuirks.hasQuirk(CameraQuirk.StickyFPS)) - setVideoMode(videoModes.get(0)); // fixes double FPS set + if (!videoModes.isEmpty()) setVideoMode(videoModes.get(0)); // fixes double FPS set } public void setAutoExposure(boolean cameraAutoExposure) { @@ -343,11 +365,27 @@ public boolean isVendorCamera() { } @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - USBCameraSource that = (USBCameraSource) o; - return cameraQuirks.equals(that.cameraQuirks); + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + USBCameraSource other = (USBCameraSource) obj; + if (camera == null) { + if (other.camera != null) return false; + } else if (!camera.equals(other.camera)) return false; + if (usbCameraSettables == null) { + if (other.usbCameraSettables != null) return false; + } else if (!usbCameraSettables.equals(other.usbCameraSettables)) return false; + if (usbFrameProvider == null) { + if (other.usbFrameProvider != null) return false; + } else if (!usbFrameProvider.equals(other.usbFrameProvider)) return false; + if (cvSink == null) { + if (other.cvSink != null) return false; + } else if (!cvSink.equals(other.cvSink)) return false; + if (cameraQuirks == null) { + if (other.cameraQuirks != null) return false; + } else if (!cameraQuirks.equals(other.cameraQuirks)) return false; + return true; } @Override 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 976053396b..483f6a7c0c 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 @@ -95,7 +95,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, // Find quirks for the current camera if (visionSource instanceof USBCameraSource) { - cameraQuirks = ((USBCameraSource) visionSource).cameraQuirks; + cameraQuirks = ((USBCameraSource) visionSource).getCameraQuirks(); } else if (visionSource instanceof LibcameraGpuSource) { cameraQuirks = QuirkyCamera.ZeroCopyPiCamera; } else { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 4b31ba0691..970b1a6ae7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -142,7 +142,15 @@ protected List tryMatchUSBCamImpl(boolean createSources) { logger.debug("Trying to match " + usbCamConfigs.size() + " unmatched configs..."); // Match camera configs to physical cameras - var matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); + + List matchedCameras; + + if (!createSources) { + matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs, false); + } else { + matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); + } + unmatchedLoadedConfigs.removeAll(matchedCameras); if (!unmatchedLoadedConfigs.isEmpty() && !hasWarned) { logger.warn( @@ -196,52 +204,128 @@ protected List tryMatchUSBCamImpl(boolean createSources) { * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. * @return the matched configurations. */ - private List matchUSBCameras( + protected List matchUSBCameras( List detectedCamInfos, List loadedUsbCamConfigs) { + return matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs, true); + } + + /** + * Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on + * disk. + * + * @param detectedCamInfos Information about currently connected USB cameras. + * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. + * @param useJNI If false, this is a unit test and the JNI should not be used for CSI devices. + * @return the matched configurations. + */ + private List matchUSBCameras( + List detectedCamInfos, + List loadedUsbCamConfigs, + boolean useJNI) { var detectedCameraList = new ArrayList<>(detectedCamInfos); ArrayList cameraConfigurations = new ArrayList<>(); - // loop over all the configs loaded from disk + List unmatchedAfterByID = new ArrayList<>(loadedUsbCamConfigs); + + // loop over all the configs loaded from disk, attempting to match each camera + // to a config by path-by-id on linux for (CameraConfiguration config : loadedUsbCamConfigs) { UsbCameraInfo cameraInfo; - // attempt matching by path and basename - logger.debug( - "Trying to find a match for loaded camera " - + config.baseName - + " with path " - + config.path); - cameraInfo = - detectedCameraList.stream() - .filter( - usbCameraInfo -> - usbCameraInfo.path.equals(config.path) - && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) - .findFirst() - .orElse(null); - - // if path based fails, attempt basename only match - if (cameraInfo == null) { - logger.debug("Failed to match by path and name, falling back to name-only match"); + if (config.otherPaths.length == 0) { + logger.debug("No valid path-by-id found for config with name " + config.baseName); + } else { + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path-by-id " + + config.otherPaths[0]); cameraInfo = detectedCameraList.stream() .filter( usbCameraInfo -> - cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + usbCameraInfo.otherPaths.length != 0 + && usbCameraInfo.otherPaths[0].equals(config.otherPaths[0]) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) .findFirst() .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByID.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } } + } + + if (!unmatchedAfterByID.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by path-by-id failed, falling back to path-only matching"); + + List unmatchedAfterByPath = new ArrayList<>(loadedUsbCamConfigs); + + // now attempt to match the cameras and configs remaining by normal path + for (CameraConfiguration config : unmatchedAfterByID) { + UsbCameraInfo cameraInfo; - // If we actually matched a camera to a config, remove that camera from the list and add it to - // the output - if (cameraInfo != null) { - logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); - detectedCameraList.remove(cameraInfo); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path " + + config.path); + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + usbCameraInfo.path.equals(config.path) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByPath.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } + + if (!unmatchedAfterByPath.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by ID and path failed, falling back to name-only matching"); + + // if both path and ID based matching fails, attempt basename only match + for (CameraConfiguration config : unmatchedAfterByPath) { + UsbCameraInfo cameraInfo; + + logger.debug("Trying to find a match for loaded camera with name " + config.baseName); + + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } } } - // If we have any unmatched cameras left, create a new CameraConfiguration for them here. + // If we have any unmatched cameras left, create a new CameraConfiguration for + // them here. logger.debug( "After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched."); for (UsbCameraInfo info : detectedCameraList) { @@ -250,7 +334,7 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) String uniqueName = baseNameToUniqueName(baseName); int suffix = 0; - while (containsName(cameraConfigurations, uniqueName)) { + while (containsName(cameraConfigurations, uniqueName) || containsName(uniqueName)) { suffix++; uniqueName = String.format("%s (%d)", uniqueName, suffix); } @@ -260,7 +344,11 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) // HACK -- for picams, we want to use the camera model String nickname = uniqueName; if (isCsiCamera(info)) { - nickname = LibCameraJNI.getSensorModel().toString(); + if (useJNI) { + nickname = LibCameraJNI.getSensorModel().toString(); + } else { + nickname = "CSICAM-DEV"; + } } CameraConfiguration configuration = @@ -282,6 +370,28 @@ private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, UsbCame logger.debug("Updating path config from " + cfg.path + " to " + info.path); cfg.path = info.path; } + cfg.otherPaths = info.otherPaths; + + if (cfg.otherPaths.length != info.otherPaths.length) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + } else { + for (int i = 0; i < info.otherPaths.length; i++) { + if (!cfg.otherPaths[i].equals(info.otherPaths[i])) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + break; + } + } + } return cfg; } @@ -292,16 +402,56 @@ public void setIgnoredCamerasRegex(String ignoredCamerasRegex) { private List filterAllowedDevices(List allDevices) { List filteredDevices = new ArrayList<>(); + List badDevices = new ArrayList<>(); + for (var device : allDevices) { + // Filter devices that are physically the same device but may show up as multiple devices that + // really cant be accessed. First noticed with raspi 5 and ov5647. + + List paths = new ArrayList<>(); + + boolean skip = false; + if (device.otherPaths.length != 0) { + // Use the other paths to filter out devices that share the same path other than the index + // select only the lowest index. + // A ov5647 on a raspi 5 would show another path as + // platform-1000880000.pisp_be-video-index0, + // platform-1000880000.pisp_be-video-index4, and platform-1000880000.pisp_be-video-index5. + // This code will remove "indexX" from all the other paths from all the devices and make + // sure + // that we only take one camera stream from each device the stream with the lowest index. + for (String p : device.otherPaths) { + paths.add(p.split("index")[0]); + } + for (var otherDevice : filteredDevices) { + if (otherDevice.otherPaths.length == 0) continue; + List otherPaths = new ArrayList<>(); + for (String p : otherDevice.otherPaths) { + otherPaths.add(p.split("index")[0]); + } + if (paths.containsAll(otherPaths)) { + if (otherDevice.dev >= device.dev) { + badDevices.add(otherDevice); + } else { + skip = true; + break; + } + } + } + } + + filteredDevices.removeAll(badDevices); if (deviceBlacklist.contains(device.name)) { logger.trace( "Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\""); } else if (device.name.matches(ignoredCamerasRegex)) { logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path); - } else { + } else if (!skip) { filteredDevices.add(device); logger.trace( "Adding local video device - \"" + device.name + "\" at \"" + device.path + "\""); + } else { + logger.trace("Skipping duplicate device: \"" + device.name + "\" at \"" + device.path); } } return filteredDevices; @@ -309,7 +459,7 @@ private List filterAllowedDevices(List allDevices) private boolean usbCamEquals(UsbCameraInfo a, UsbCameraInfo b) { return a.path.equals(b.path) - && a.dev == b.dev + // && a.dev == b.dev (dev is not constant in Windows) && a.name.equals(b.name) && a.productId == b.productId && a.vendorId == b.vendorId; @@ -342,7 +492,7 @@ private static List loadVisionSourcesFromCamConfigs( cameraSources.add(piCamSrc); } else { var newCam = new USBCameraSource(configuration); - if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken) + if (!newCam.getCameraQuirks().hasQuirk(CameraQuirk.CompletelyBroken) && !newCam.getSettables().videoModes.isEmpty()) { cameraSources.add(newCam); } @@ -363,4 +513,15 @@ private boolean containsName( return configList.stream() .anyMatch(configuration -> configuration.uniqueName.equals(uniqueName)); } + + /** + * Check if the current list of known cameras contains the given unique name. + * + * @param uniqueName The unique name. + * @return If the list of cameras contains the unique name. + */ + private boolean containsName(final String uniqueName) { + return VisionModuleManager.getInstance().getModules().stream() + .anyMatch(camera -> camera.visionSource.cameraConfiguration.uniqueName.equals(uniqueName)); + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index cda7d08e6f..f2057a18db 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -59,7 +59,7 @@ public void setBlueGain(int blue) {} public abstract VideoMode getCurrentVideoMode(); public void setVideoModeInternal(int index) { - setVideoMode(getAllVideoModes().get(index)); + if (!getAllVideoModes().isEmpty()) setVideoMode(getAllVideoModes().get(index)); } public void setVideoMode(VideoMode mode) { diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java index 1cd3579382..6d1e6985e7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -16,6 +16,7 @@ */ package org.photonvision.vision.target; +import com.fasterxml.jackson.annotation.JsonAlias; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; @@ -108,6 +109,7 @@ public enum TargetModel implements Releasable { -Units.inchesToMeters(9.5) / 2)), 0), // 2023 AprilTag, with 6 inch marker width (inner black square). + @JsonAlias({"k6in_16h5"}) kAprilTag6in_16h5( // Corners of the tag's inner black square (excluding white border) List.of( @@ -117,6 +119,7 @@ public enum TargetModel implements Releasable { new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), Units.inchesToMeters(3 * 2)), // 2024 AprilTag, with 6.5 inch marker width (inner black square). + @JsonAlias({"k6p5in_36h11", "k200mmAprilTag", "kAruco6p5in_36h11"}) kAprilTag6p5in_36h11( // Corners of the tag's inner black square (excluding white border) List.of( diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java index beaae0e4d9..ad6db6fd12 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java @@ -31,6 +31,7 @@ import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.vision.pipeline.AprilTagPipelineSettings; +import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.ColoredShapePipelineSettings; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.target.TargetModel; @@ -132,13 +133,34 @@ public static void cleanup() throws IOException { public void testJacksonHandlesOldVersions() throws IOException { var str = "{\"baseName\":\"aaaaaa\",\"uniqueName\":\"aaaaaa\",\"nickname\":\"aaaaaa\",\"FOV\":70.0,\"path\":\"dev/vid\",\"cameraType\":\"UsbCamera\",\"currentPipelineIndex\":0,\"camPitch\":{\"radians\":0.0},\"calibrations\":[], \"cameraLEDs\":[]}"; - var writer = new FileWriter("test.json"); + File tempFile = File.createTempFile("test", ".json"); + tempFile.deleteOnExit(); + var writer = new FileWriter(tempFile); writer.write(str); writer.flush(); writer.close(); Assertions.assertDoesNotThrow( - () -> JacksonUtils.deserialize(Path.of("test.json"), CameraConfiguration.class)); + () -> JacksonUtils.deserialize(tempFile.toPath(), CameraConfiguration.class)); - new File("test.json").delete(); + tempFile.delete(); + } + + @Test + public void testJacksonHandlesOldTargetEnum() throws IOException { + var str = "[ \"AprilTagPipelineSettings\", {\n \"targetModel\" : \"k6in_16h5\"\n} ]\n"; + + File tempFile = File.createTempFile("test", ".json"); + tempFile.deleteOnExit(); + var writer = new FileWriter(tempFile); + writer.write(str); + writer.flush(); + writer.close(); + + AprilTagPipelineSettings settings = + (AprilTagPipelineSettings) + JacksonUtils.deserialize(tempFile.toPath(), CVPipelineSettings.class); + Assertions.assertEquals(TargetModel.kAprilTag6in_16h5, settings.targetModel); + + tempFile.delete(); } } diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index e0a8376159..9b4ca528b6 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -31,6 +31,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.util.TestUtils; +import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.frame.provider.FileFrameProvider; @@ -165,7 +166,16 @@ public void testMultipleStreamIndex() { TestUtils.WPI2019Image.FOV); var testSource3 = new TestSource(ffp3, conf3); - var modules = vmm.addSources(List.of(testSource, testSource2, testSource3)); + // Arducam OV9281 UC844 raspberry pi test. + var conf4 = new CameraConfiguration("Left", "dev/video1"); + USBCameraSource usbSimulation = new USBCameraSource(conf4, 0x6366, 0x0c45, true); + + var conf5 = new CameraConfiguration("Right", "dev/video2"); + USBCameraSource usbSimulation2 = new USBCameraSource(conf5, 0x6366, 0x0c45, true); + + var modules = + vmm.addSources( + List.of(testSource, testSource2, testSource3, usbSimulation, usbSimulation2)); System.out.println( Arrays.toString( @@ -176,9 +186,15 @@ public void testMultipleStreamIndex() { modules.stream() .map(it -> it.visionSource.getCameraConfiguration().streamIndex) .collect(Collectors.toList()); + + assertTrue(usbSimulation.equals(usbSimulation)); + assertTrue(!usbSimulation.equals(usbSimulation2)); + assertTrue(idxs.contains(0)); assertTrue(idxs.contains(1)); assertTrue(idxs.contains(2)); + assertTrue(idxs.contains(3)); + assertTrue(idxs.contains(4)); } private static void printTestResults(CVPipelineResult pipelineResult) { diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index 2c64a9a622..4a995d26fd 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -35,23 +35,165 @@ public void visionSourceTest() { ConfigManager.getInstance().load(); inst.tryMatchUSBCamImpl(); - var config = new CameraConfiguration("secondTestVideo", "dev/video1"); + + var config3 = + new CameraConfiguration( + "thirdTestVideo", + "thirdTestVideo", + "thirdTestVideo", + "dev/video1", + new String[] {"by-id/123"}); + var config4 = + new CameraConfiguration( + "fourthTestVideo", + "fourthTestVideo", + "fourthTestVideo", + "dev/video2", + new String[] {"by-id/321"}); + UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); + infoList.add(info1); - inst.registerLoadedConfigs(config); - var sources = inst.tryMatchUSBCamImpl(false); + inst.registerLoadedConfigs(config3, config4); + + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info1)); - assertEquals(1, inst.unmatchedLoadedConfigs.size()); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); UsbCameraInfo info2 = - new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 1); + new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3); + infoList.add(info2); + + var cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + // assertEquals("testVideo (1)", cams.get(0).uniqueName); // Proper suffixing + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info2)); - assertEquals(2, inst.knownUsbCameras.size()); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info3 = + new UsbCameraInfo(0, "dev/video2", "thirdTestVideo", new String[] {"by-id/123"}, 3, 4); + + UsbCameraInfo info4 = + new UsbCameraInfo(0, "dev/video3", "fourthTestVideo", new String[] {"by-id/321"}, 5, 6); + + infoList.add(info4); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + var cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + // If this is null, cam4 got matched to config3 instead of config4 + + assertEquals(cam4.nickname, config4.nickname); + + infoList.add(info3); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info2)); + assertTrue(inst.knownUsbCameras.contains(info3)); + + var cam3 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config3.otherPaths[0])) + .findFirst() + .orElse(null); + cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + + assertEquals(cam3.nickname, config3.nickname); + assertEquals(cam4.nickname, config4.nickname); + + UsbCameraInfo info5 = + new UsbCameraInfo( + 2, + "/dev/video2", + "Left Camera", + new String[] { + "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Left_Camera_12345-video-index0", + "/dev/v4l/by-path/platform-xhci-hcd.0-usb-0:2:1.0-video-index0" + }, + 7, + 8); + infoList.add(info5); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info5)); + + UsbCameraInfo info6 = + new UsbCameraInfo( + 3, + "dev/video3", + "Right Camera", + new String[] { + "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Right_Camera_123456-video-index0", + "/dev/v4l/by-path/platform-xhci-hcd.1-usb-0:1:1.0-video-index0" + }, + 9, + 10); + infoList.add(info6); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info6)); + + // RPI 5 CSI Tests + + UsbCameraInfo info7 = + new UsbCameraInfo( + 4, + "dev/video4", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index0"}, + 11, + 12); + infoList.add(info7); + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info7)); + + UsbCameraInfo info8 = + new UsbCameraInfo( + 5, + "dev/video8", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index4"}, + 13, + 14); + infoList.add(info8); + inst.tryMatchUSBCamImpl(false); + + assertTrue(!inst.knownUsbCameras.contains(info8)); // This camera should not be recognized/used. + + UsbCameraInfo info9 = + new UsbCameraInfo( + 6, + "dev/video9", + "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV + new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index5"}, + 15, + 16); + infoList.add(info9); + inst.tryMatchUSBCamImpl(false); + + assertTrue(!inst.knownUsbCameras.contains(info9)); // This camera should not be recognized/used. + assertEquals(7, inst.knownUsbCameras.size()); assertEquals(0, inst.unmatchedLoadedConfigs.size()); } } diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index e5d880fb4a..099308f470 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -9,54 +9,14 @@ import java.nio.file.Path ext { nativeName = "photonlib" includePhotonTargeting = true + // Include the generated Version file + generatedHeaders = "src/generate/native/include" } -apply from: "${rootDir}/shared/setupBuild.gradle" +apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" apply from: "${rootDir}/versioningHelper.gradle" - -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() - -def nativeConfigName = 'wpilibNatives' -def nativeConfig = configurations.create(nativeConfigName) - -def nativeTasks = wpilibTools.createExtractionTasks { - configurationName = nativeConfigName -} - -nativeTasks.addToSourceSetResources(sourceSets.main) - -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") -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" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - -dependencies { - implementation project(":photon-targeting") - - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("cameraserver") - implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - - implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() - - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); -} - +// Include the version file in the distributed sources cppHeadersZip { from('src/generate/native/include') { into '/' @@ -66,8 +26,6 @@ cppHeadersZip { def photonlibFileInput = file("src/generate/photonlib.json.in") ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json") -def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) - task generateVendorJson() { description = "Generates the vendor JSON file" group = "PhotonVision" @@ -103,18 +61,9 @@ task writeCurrentVersion { build.mustRunAfter writeCurrentVersion -model { - components { - all { - it.sources.each { - it.exportedHeaders { - srcDirs "src/main/native/include" - srcDirs "src/generate/native/include" - } - } - } - } +def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) +model { // Publish the vendordep json publishing { publications { diff --git a/photon-lib/py/.gitignore b/photon-lib/py/.gitignore new file mode 100644 index 0000000000..f0c4ced6d6 --- /dev/null +++ b/photon-lib/py/.gitignore @@ -0,0 +1,5 @@ +photonlibpy.egg-info/ +dist/ +build/ +.eggs/ +photonlibpy/version.py diff --git a/photon-lib/py/buildAndTest.bat b/photon-lib/py/buildAndTest.bat new file mode 100644 index 0000000000..b9774a54d6 --- /dev/null +++ b/photon-lib/py/buildAndTest.bat @@ -0,0 +1,14 @@ +:: Uninstall if it already was installed +pip uninstall -y photonlibpy + +:: Build wheel +python setup.py bdist_wheel + +:: Install whatever wheel was made +for %%f in (dist/*.whl) do ( + echo installing dist/%%f + pip install --no-cache-dir dist/%%f +) + +:: Run the test suite +pytest diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py new file mode 100644 index 0000000000..d5b258f5e7 --- /dev/null +++ b/photon-lib/py/photonlibpy/__init__.py @@ -0,0 +1 @@ +# No one here but us chickens diff --git a/photon-lib/py/photonlibpy/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/multiTargetPNPResult.py new file mode 100644 index 0000000000..63bbb34301 --- /dev/null +++ b/photon-lib/py/photonlibpy/multiTargetPNPResult.py @@ -0,0 +1,45 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class PNPResult: + _NUM_BYTES_IN_FLOAT = 8 + PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3) + + isPresent: bool = False + best: Transform3d = field(default_factory=Transform3d) + alt: Transform3d = field(default_factory=Transform3d) + ambiguity: float = 0.0 + bestReprojError: float = 0.0 + altReprojError: float = 0.0 + + def createFromPacket(self, packet: Packet) -> Packet: + self.isPresent = packet.decodeBoolean() + self.best = packet.decodeTransform() + self.alt = packet.decodeTransform() + self.bestReprojError = packet.decodeDouble() + self.altReprojError = packet.decodeDouble() + self.ambiguity = packet.decodeDouble() + return packet + + +@dataclass +class MultiTargetPNPResult: + _MAX_IDS = 32 + # pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) + _PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS) + + estimatedPose: PNPResult = field(default_factory=PNPResult) + fiducialIDsUsed: list[int] = field(default_factory=list) + + def createFromPacket(self, packet: Packet) -> Packet: + self.estimatedPose = PNPResult() + self.estimatedPose.createFromPacket(packet) + self.fiducialIDsUsed = [] + for _ in range(MultiTargetPNPResult._MAX_IDS): + fidId = packet.decode16() + if fidId >= 0: + self.fiducialIDsUsed.append(fidId) + return packet diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py new file mode 100644 index 0000000000..c3816d94f8 --- /dev/null +++ b/photon-lib/py/photonlibpy/packet.py @@ -0,0 +1,143 @@ +import struct +from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion +import wpilib + + +class Packet: + def __init__(self, data: list[int]): + """ + * Constructs an empty packet. + * + * @param self.size The self.size of the packet buffer. + """ + self.packetData = data + self.size = len(data) + self.readPos = 0 + self.outOfBytes = False + + def clear(self): + """Clears the packet and resets the read and write positions.""" + self.packetData = [0] * self.size + self.readPos = 0 + self.outOfBytes = False + + def getSize(self): + return self.size + + _NO_MORE_BYTES_MESSAGE = """ + Photonlib - Ran out of bytes while decoding. + Make sure the version of photonvision on the coprocessor + matches the version of photonlib running in the robot code. + """ + + def _getNextByte(self) -> int: + retVal = 0x00 + + if not self.outOfBytes: + try: + retVal = 0x00FF & self.packetData[self.readPos] + self.readPos += 1 + except IndexError: + wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True) + self.outOfBytes = True + + return retVal + + def getData(self) -> list[int]: + """ + * Returns the packet data. + * + * @return The packet data. + """ + return self.packetData + + def setData(self, data: list[int]): + """ + * Sets the packet data. + * + * @param data The packet data. + """ + self.clear() + self.packetData = data + self.size = len(self.packetData) + + def _decodeGeneric(self, unpackFormat, numBytes): + # Read ints in from the data buffer + intList = [] + for _ in range(numBytes): + intList.append(self._getNextByte()) + + # Interpret the bytes as a floating point number + value = struct.unpack(unpackFormat, bytes(intList))[0] + + return value + + def decode8(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">b", 1) + + def decode16(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">h", 2) + + def decode32(self) -> int: + """ + * Returns a decoded int (32 bytes) from the packet. + * + * @return A decoded int from the packet. + """ + return self._decodeGeneric(">l", 4) + + def decodeDouble(self) -> float: + """ + * Returns a decoded double from the packet. + * + * @return A decoded double from the packet. + """ + return self._decodeGeneric(">d", 8) + + def decodeBoolean(self) -> bool: + """ + * Returns a decoded boolean from the packet. + * + * @return A decoded boolean from the packet. + """ + return self.decode8() == 1 + + def decodeDoubleArray(self, length: int) -> list[float]: + """ + * Returns a decoded array of floats from the packet. + * + * @return A decoded array of floats from the packet. + """ + ret = [] + for _ in range(length): + ret.append(self.decodeDouble()) + return ret + + def decodeTransform(self) -> Transform3d: + """ + * Returns a decoded Transform3d + * + * @return A decoded Tansform3d from the packet. + """ + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + translation = Translation3d(x, y, z) + + w = self.decodeDouble() + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + rotation = Rotation3d(Quaternion(w, x, y, z)) + + return Transform3d(translation, rotation) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py new file mode 100644 index 0000000000..fd3e0c8660 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -0,0 +1,175 @@ +from enum import Enum +import ntcore +from wpilib import Timer +import wpilib +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION + + +class VisionLEDMode(Enum): + kDefault = -1 + kOff = 0 + kOn = 1 + kBlink = 2 + + +lastVersionTimeCheck = 0.0 +_VERSION_CHECK_ENABLED = True + + +def setVersionCheckEnabled(enabled: bool): + global _VERSION_CHECK_ENABLED + _VERSION_CHECK_ENABLED = enabled + + +class PhotonCamera: + def __init__(self, cameraName: str): + instance = ntcore.NetworkTableInstance.getDefault() + self.name = cameraName + self._tableName = "photonvision" + photonvision_root_table = instance.getTable(self._tableName) + self.cameraTable = photonvision_root_table.getSubTable(cameraName) + self.path = self.cameraTable.getPath() + self.rawBytesEntry = self.cameraTable.getRawTopic("rawBytes").subscribe( + "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True) + ) + + self.driverModePublisher = self.cameraTable.getBooleanTopic( + "driverModeRequest" + ).publish() + self.driverModeSubscriber = self.cameraTable.getBooleanTopic( + "driverMode" + ).subscribe(False) + self.inputSaveImgEntry = self.cameraTable.getIntegerTopic( + "inputSaveImgCmd" + ).getEntry(0) + self.outputSaveImgEntry = self.cameraTable.getIntegerTopic( + "outputSaveImgCmd" + ).getEntry(0) + self.pipelineIndexRequest = self.cameraTable.getIntegerTopic( + "pipelineIndexRequest" + ).publish() + self.pipelineIndexState = self.cameraTable.getIntegerTopic( + "pipelineIndexState" + ).subscribe(0) + self.heartbeatEntry = self.cameraTable.getIntegerTopic("heartbeat").subscribe( + -1 + ) + + self.ledModeRequest = photonvision_root_table.getIntegerTopic( + "ledModeRequest" + ).publish() + self.ledModeState = photonvision_root_table.getIntegerTopic( + "ledModeState" + ).subscribe(-1) + self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe( + "" + ) + + # Existing is enough to make this multisubscriber do its thing + self.topicNameSubscriber = ntcore.MultiSubscriber( + instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True) + ) + + self.prevHeartbeat = 0 + self.prevHeartbeatChangeTime = Timer.getFPGATimestamp() + + def getLatestResult(self) -> PhotonPipelineResult: + self._versionCheck() + + retVal = PhotonPipelineResult() + packetWithTimestamp = self.rawBytesEntry.getAtomic() + byteList = packetWithTimestamp.value + timestamp = packetWithTimestamp.time + + if len(byteList) < 1: + return retVal + else: + retVal.populateFromPacket(Packet(byteList)) + # NT4 allows us to correct the timestamp based on when the message was sent + retVal.setTimestampSeconds( + timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3 + ) + return retVal + + def getDriverMode(self) -> bool: + return self.driverModeSubscriber.get() + + def setDriverMode(self, driverMode: bool) -> None: + self.driverModePublisher.set(driverMode) + + def takeInputSnapshot(self) -> None: + self.inputSaveImgEntry.set(self.inputSaveImgEntry.get() + 1) + + def takeOutputSnapshot(self) -> None: + self.outputSaveImgEntry.set(self.outputSaveImgEntry.get() + 1) + + def getPipelineIndex(self) -> int: + return self.pipelineIndexState.get(0) + + def setPipelineIndex(self, index: int) -> None: + self.pipelineIndexRequest.set(index) + + def getLEDMode(self) -> VisionLEDMode: + mode = self.ledModeState.get() + return VisionLEDMode(mode) + + def setLEDMode(self, led: VisionLEDMode) -> None: + self.ledModeRequest.set(led.value) + + def getName(self) -> str: + return self.name + + def isConnected(self) -> bool: + curHeartbeat = self.heartbeatEntry.get() + now = Timer.getFPGATimestamp() + + if curHeartbeat != self.prevHeartbeat: + self.prevHeartbeat = curHeartbeat + self.prevHeartbeatChangeTime = now + + return (now - self.prevHeartbeatChangeTime) < 0.5 + + def _versionCheck(self) -> None: + global lastVersionTimeCheck + + if not _VERSION_CHECK_ENABLED: + return + + if (Timer.getFPGATimestamp() - lastVersionTimeCheck) < 5.0: + return + + lastVersionTimeCheck = Timer.getFPGATimestamp() + + if not self.heartbeatEntry.exists(): + cameraNames = ( + self.cameraTable.getInstance().getTable(self._tableName).getSubTables() + ) + if len(cameraNames) == 0: + wpilib.reportError( + "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", + False, + ) + else: + wpilib.reportError( + f"PhotonVision coprocessor at path {self.path} not found in Network Tables. Double check that your camera names match! Only the following camera names were found: { ''.join(cameraNames)}", + True, + ) + + elif not self.isConnected(): + wpilib.reportWarning( + f"PhotonVision coprocessor at path {self.path} is not sending new data.", + True, + ) + + versionString = self.versionEntry.get(defaultValue="") + if len(versionString) > 0 and versionString != PHOTONVISION_VERSION: + wpilib.reportWarning( + "Photon version " + + PHOTONVISION_VERSION + + " does not match coprocessor version " + + versionString + + f"! Please install photonlibpy version {PHOTONLIB_VERSION}", + True, + ) diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py new file mode 100644 index 0000000000..3fe02c9fea --- /dev/null +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -0,0 +1,38 @@ +from dataclasses import dataclass, field + +from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult +from photonlibpy.packet import Packet +from photonlibpy.photonTrackedTarget import PhotonTrackedTarget + + +@dataclass +class PhotonPipelineResult: + latencyMillis: float = -1.0 + timestampSec: float = -1.0 + targets: list[PhotonTrackedTarget] = field(default_factory=list) + multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult) + + def populateFromPacket(self, packet: Packet) -> Packet: + self.targets = [] + self.latencyMillis = packet.decodeDouble() + self.multiTagResult = MultiTargetPNPResult() + self.multiTagResult.createFromPacket(packet) + targetCount = packet.decode8() + for _ in range(targetCount): + target = PhotonTrackedTarget() + target.createFromPacket(packet) + self.targets.append(target) + + return packet + + def setTimestampSeconds(self, timestampSec: float) -> None: + self.timestampSec = timestampSec + + def getLatencyMillis(self) -> float: + return self.latencyMillis + + def getTimestamp(self) -> float: + return self.timestampSec + + def getTargets(self) -> list[PhotonTrackedTarget]: + return self.targets diff --git a/photon-lib/py/photonlibpy/photonTrackedTarget.py b/photon-lib/py/photonlibpy/photonTrackedTarget.py new file mode 100644 index 0000000000..56c225d997 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonTrackedTarget.py @@ -0,0 +1,82 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class TargetCorner: + x: float + y: float + + +@dataclass +class PhotonTrackedTarget: + _MAX_CORNERS = 8 + _NUM_BYTES_IN_FLOAT = 8 + _PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS) + + yaw: float = 0.0 + pitch: float = 0.0 + area: float = 0.0 + skew: float = 0.0 + fiducialId: int = -1 + bestCameraToTarget: Transform3d = field(default_factory=Transform3d) + altCameraToTarget: Transform3d = field(default_factory=Transform3d) + minAreaRectCorners: list[TargetCorner] | None = None + detectedCorners: list[TargetCorner] | None = None + poseAmbiguity: float = 0.0 + + def getYaw(self) -> float: + return self.yaw + + def getPitch(self) -> float: + return self.pitch + + def getArea(self) -> float: + return self.area + + def getSkew(self) -> float: + return self.skew + + def getFiducialId(self) -> int: + return self.fiducialId + + def getPoseAmbiguity(self) -> float: + return self.poseAmbiguity + + def getMinAreaRectCorners(self) -> list[TargetCorner] | None: + return self.minAreaRectCorners + + def getDetectedCorners(self) -> list[TargetCorner] | None: + return self.detectedCorners + + def getBestCameraToTarget(self) -> Transform3d: + return self.bestCameraToTarget + + def getAlternateCameraToTarget(self) -> Transform3d: + return self.altCameraToTarget + + def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]: + retList = [] + for _ in range(numTargets): + cx = packet.decodeDouble() + cy = packet.decodeDouble() + retList.append(TargetCorner(cx, cy)) + return retList + + def createFromPacket(self, packet: Packet) -> Packet: + self.yaw = packet.decodeDouble() + self.pitch = packet.decodeDouble() + self.area = packet.decodeDouble() + self.skew = packet.decodeDouble() + self.fiducialId = packet.decode32() + + self.bestCameraToTarget = packet.decodeTransform() + self.altCameraToTarget = packet.decodeTransform() + + self.poseAmbiguity = packet.decodeDouble() + + self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four + numCorners = packet.decode8() + self.detectedCorners = self._decodeTargetList(packet, numCorners) + return packet diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py new file mode 100644 index 0000000000..b1c3a06133 --- /dev/null +++ b/photon-lib/py/setup.py @@ -0,0 +1,54 @@ +from setuptools import setup, find_packages +import subprocess, re + +gitDescribeResult = ( + subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"]) + .decode("utf-8") + .strip() +) + +m = re.search( + r"(v[0-9]{4}\.[0-9]{1}\.[0-9]{1})-?((?:beta)?(?:alpha)?)-?([0-9\.]*)", + gitDescribeResult, +) + +# Extract the first portion of the git describe result +# which should be PEP440 compliant +if m: + versionString = m.group(0) + prefix = m.group(1) + maturity = m.group(2) + suffix = m.group(3).replace(".", "") + versionString = f"{prefix}.{maturity}.{suffix}" + + +else: + print("Warning, no valid version found") + versionString = gitDescribeResult + +print(f"Building version {versionString}") + +# Put the version info into a python file for runtime access +with open("photonlibpy/version.py", "w", encoding="utf-8") as fp: + fp.write(f'PHOTONLIB_VERSION="{versionString}"\n') + fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n') + + +descriptionStr = f""" +Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. +Implemented with PhotonVision version {gitDescribeResult} . +""" + +setup( + name="photonlibpy", + packages=find_packages(), + version=versionString, + install_requires=[ + "wpilib<2025,>=2024.0.0b2", + "robotpy-wpimath<2025,>=2024.0.0b2", + "pyntcore<2025,>=2024.0.0b2", + ], + description=descriptionStr, + url="https://photonvision.org", + author="Photonvision Development Team", +) diff --git a/photon-lib/py/test/data.py b/photon-lib/py/test/data.py new file mode 100644 index 0000000000..8c317dfe2f --- /dev/null +++ b/photon-lib/py/test/data.py @@ -0,0 +1,239 @@ +# fmt: off +rawBytes1 = [ + 64, 166, 117, 41, 225, 243, 165, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes2 = [ + 64, 114, 72, 58, 227, 96, 141, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 0 +] +rawBytes3 = [ + 64, 55, 65, 189, 215, 102, 131, 195, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes4 = [ + 64, 115, 23, 245, 248, 9, 145, 121, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 41, 32, 212, 70, 53, 253, 38, 64, 19, 140, + 198, 187, 206, 56, 251, 64, 38, 63, 170, 170, 170, 170, 170, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 17, 63, 226, 15, 19, 123, 229, 152, 236, 191, 191, 225, + 27, 96, 1, 219, 30, 63, 168, 175, 103, 65, 172, 24, 8, 63, 115, 51, 123, + 216, 202, 14, 128, 191, 155, 163, 119, 215, 217, 209, 224, 63, 212, 76, 79, + 227, 166, 197, 80, 63, 238, 85, 211, 252, 64, 132, 136, 63, 226, 13, 126, + 170, 219, 202, 209, 191, 190, 134, 80, 94, 25, 179, 17, 63, 168, 92, 53, + 102, 36, 28, 64, 63, 204, 203, 52, 12, 186, 226, 51, 63, 148, 67, 104, 89, + 131, 114, 208, 63, 211, 104, 18, 25, 149, 138, 78, 63, 237, 159, 242, 53, + 211, 204, 51, 63, 217, 254, 169, 82, 190, 36, 22, 64, 123, 96, 0, 6, 100, + 53, 178, 64, 112, 207, 255, 241, 198, 25, 18, 64, 132, 215, 255, 254, 189, + 61, 109, 64, 86, 191, 255, 217, 164, 214, 161, 64, 138, 55, 94, 60, 205, + 229, 39, 64, 115, 130, 222, 78, 57, 230, 238, 64, 131, 15, 94, 65, 66, 194, + 147, 64, 126, 162, 222, 73, 150, 202, 88, 4, 64, 130, 248, 64, 192, 0, 0, 0, + 64, 126, 15, 133, 64, 0, 0, 0, 64, 137, 206, 237, 128, 0, 0, 0, 64, 116, 48, + 240, 32, 0, 0, 0, 64, 132, 218, 43, 96, 0, 0, 0, 64, 86, 210, 155, 128, 0, + 0, 0, 64, 123, 102, 127, 192, 0, 0, 0, 64, 112, 211, 233, 96, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] +rawBytes5 = [ + 64, 102, 149, 235, 181, 90, 192, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 21, 210, 112, 148, 86, 4, 131, 64, 3, 87, + 196, 18, 174, 105, 145, 64, 47, 80, 237, 9, 123, 66, 95, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 17, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, + 0, 0, 0, 0, 64, 116, 192, 0, 0, 0, 0, 0, 64, 127, 128, 0, 0, 0, 0, 0, 64, + 116, 192, 0, 0, 0, 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, + 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, 0, 0, 64, 127, 128, + 0, 0, 0, 0, 0, 4, 64, 133, 78, 45, 224, 0, 0, 0, 64, 127, 129, 184, 160, 0, + 0, 0, 64, 133, 78, 1, 192, 0, 0, 0, 64, 100, 100, 194, 224, 0, 0, 0, 64, + 118, 181, 224, 64, 0, 0, 0, 64, 102, 98, 136, 0, 0, 0, 0, 64, 116, 207, 155, + 64, 0, 0, 0, 64, 126, 121, 100, 96, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0 +] +rawBytes6 = [ + 64, 78, 129, 235, 32, 116, 234, 142, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 8, 64, 43, 213, 73, 8, 241, 221, 240, 192, 2, 146, + 71, 190, 201, 205, 25, 64, 37, 96, 141, 183, 156, 102, 0, 192, 9, 112, 76, + 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 101, 10, 36, 92, 234, 132, 108, 64, 89, 45, 24, + 60, 197, 216, 218, 64, 114, 7, 35, 134, 17, 204, 20, 64, 87, 129, 247, 219, + 201, 12, 226, 64, 114, 67, 177, 81, 138, 189, 202, 64, 100, 68, 236, 33, + 157, 19, 147, 64, 101, 131, 63, 243, 220, 103, 216, 64, 101, 26, 124, 82, + 27, 121, 143, 0, 192, 32, 131, 181, 181, 155, 145, 13, 192, 37, 92, 235, 61, + 221, 83, 253, 63, 173, 176, 233, 61, 133, 212, 255, 192, 73, 171, 139, 128, + 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 92, 42, 37, 190, 203, 146, 101, 64, 102, 61, 168, + 153, 137, 73, 186, 64, 94, 48, 100, 45, 88, 178, 51, 64, 100, 249, 193, 146, + 50, 77, 24, 64, 94, 220, 25, 65, 52, 109, 155, 64, 101, 62, 112, 102, 118, + 182, 70, 64, 92, 213, 218, 210, 167, 77, 205, 64, 102, 130, 87, 109, 205, + 178, 232, 0, 192, 19, 156, 59, 67, 88, 173, 35, 192, 37, 85, 193, 76, 240, + 22, 41, 63, 152, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, + 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, + 96, 128, 0, 0, 0, 0, 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, + 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, 0, 64, 101, 224, 0, + 0, 0, 0, 0, 64, 96, 128, 0, 0, 0, 0, 0, 64, 101, 224, 0, 0, 0, 0, 0, 0, 64, + 48, 164, 171, 25, 83, 4, 154, 192, 34, 178, 85, 3, 174, 51, 62, 63, 147, + 255, 255, 245, 245, 4, 85, 192, 28, 128, 4, 0, 0, 0, 0, 255, 255, 255, 255, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, + 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 110, + 24, 157, 164, 237, 183, 149, 64, 101, 4, 236, 103, 73, 0, 224, 64, 111, 24, + 157, 165, 1, 191, 69, 64, 100, 228, 236, 103, 153, 126, 38, 64, 111, 32, 0, + 27, 18, 72, 107, 64, 101, 32, 0, 24, 182, 255, 32, 64, 110, 32, 0, 26, 254, + 64, 187, 64, 101, 64, 0, 24, 102, 129, 218, 0, 63, 247, 149, 178, 38, 100, + 246, 86, 192, 9, 184, 134, 230, 194, 222, 110, 63, 150, 102, 102, 85, 138, + 188, 43, 64, 81, 228, 41, 192, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 100, 147, 51, 63, + 205, 146, 189, 64, 96, 153, 153, 160, 125, 112, 225, 64, 100, 224, 0, 12, + 123, 135, 101, 64, 96, 128, 0, 6, 164, 45, 139, 64, 101, 35, 51, 64, 50, + 109, 67, 64, 97, 73, 153, 159, 130, 143, 31, 64, 100, 214, 102, 115, 132, + 120, 155, 64, 97, 99, 51, 57, 91, 210, 117, 0, 192, 33, 131, 192, 140, 90, + 220, 117, 192, 54, 53, 227, 123, 209, 81, 175, 63, 136, 0, 0, 0, 0, 0, 0, + 64, 86, 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 92, 128, 0, 0, 0, 0, 0, 64, + 109, 128, 0, 0, 0, 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 128, 0, 0, 0, + 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 64, 92, 128, + 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 0, 192, 25, 145, 202, 146, 13, + 244, 248, 192, 36, 180, 8, 18, 34, 149, 42, 63, 128, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, 64, 101, 96, 0, + 0, 0, 0, 0, 64, 96, 32, 0, 0, 0, 0, 0, 64, 101, 96, 0, 0, 0, 0, 0, 64, 96, + 32, 0, 0, 0, 0, 0, 64, 101, 160, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, + 64, 101, 160, 0, 0, 0, 0, 0, 0, 192, 35, 144, 241, 16, 205, 7, 236, 192, 0, + 254, 21, 96, 184, 112, 69, 63, 135, 255, 255, 219, 151, 33, 85, 192, 70, + 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 91, 0, 0, 64, 109, 58, 156, 64, 96, + 96, 0, 31, 237, 203, 144, 64, 91, 192, 0, 63, 219, 151, 34, 64, 96, 0, 0, + 32, 54, 157, 78, 64, 92, 32, 0, 63, 146, 197, 100, 64, 96, 48, 0, 32, 18, + 52, 112, 64, 91, 96, 0, 64, 36, 104, 222, 64, 96, 144, 0, 31, 201, 98, 178, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py new file mode 100644 index 0000000000..92b1636351 --- /dev/null +++ b/photon-lib/py/test/photonlibpy_test.py @@ -0,0 +1,46 @@ +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from data import rawBytes1 +from data import rawBytes2 +from data import rawBytes3 +from data import rawBytes4 +from data import rawBytes5 +from data import rawBytes6 + + +def setupCommon(bytesIn): + res = PhotonPipelineResult() + packet = Packet(bytesIn) + res.populateFromPacket(packet) + assert packet.outOfBytes is False + return res + + +def test_byteParse1(): + res = setupCommon(rawBytes1) + assert len(res.getTargets()) == 0 + + +def test_byteParse2(): + res = setupCommon(rawBytes2) + assert len(res.getTargets()) == 0 + + +def test_byteParse3(): + res = setupCommon(rawBytes3) + assert len(res.getTargets()) == 0 + + +def test_byteParse4(): + res = setupCommon(rawBytes4) + assert len(res.getTargets()) == 1 + + +def test_byteParse5(): + res = setupCommon(rawBytes5) + assert len(res.getTargets()) == 1 + + +def test_byteParse6(): + res = setupCommon(rawBytes6) + assert len(res.getTargets()) > 6 diff --git a/photon-lib/src/main/driver/cpp/VendorJNI.cpp b/photon-lib/src/main/driver/cpp/VendorJNI.cpp deleted file mode 100644 index c449ee05c9..0000000000 --- a/photon-lib/src/main/driver/cpp/VendorJNI.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "com_vendor_jni_VendorJNI.h" -#include "jni.h" - -JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { - // Check to ensure the JNI version is valid - - JNIEnv* env; - if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) - return JNI_ERR; - - // In here is also where you store things like class references - // if they are ever needed - - return JNI_VERSION_1_6; -} - -JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {} - -/* - * Class: com_vendor_jni_VendorJNI - * Method: initialize - * Signature: ()I - */ -JNIEXPORT jint JNICALL -Java_com_vendor_jni_VendorJNI_initialize - (JNIEnv*, jclass) -{ - return 0; -} diff --git a/photon-lib/src/main/driver/symbols.txt b/photon-lib/src/main/driver/symbols.txt deleted file mode 100644 index 2f9b64148a..0000000000 --- a/photon-lib/src/main/driver/symbols.txt +++ /dev/null @@ -1,4 +0,0 @@ -JNI_OnLoad -JNI_OnUnload -Java_com_vendor_jni_VendorJNI_initialize -c_doThing diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 8b46a2c372..faa03ebc54 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,6 +24,8 @@ package org.photonvision; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -52,6 +54,7 @@ /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { + private static int InstanceCount = 0; public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; @@ -152,6 +155,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); + + HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; } /** @@ -321,14 +327,14 @@ public boolean isConnected() { public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); } else return Optional.empty(); } public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); } else return Optional.empty(); } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index dbf36292c2..f93c499140 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -25,6 +25,8 @@ package org.photonvision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -52,6 +54,8 @@ * below. Example usage can be found in our apriltagExample example project. */ public class PhotonPoseEstimator { + private static int InstanceCount = 0; + /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */ public enum PoseStrategy { /** Choose the Pose with the lowest ambiguity. */ @@ -118,14 +122,14 @@ public PhotonPoseEstimator( this.primaryStrategy = strategy; this.camera = camera; this.robotToCamera = robotToCamera; + + HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); + InstanceCount++; } public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = null; - this.robotToCamera = robotToCamera; + this(fieldTags, strategy, null, robotToCamera); } /** Invalidates the pose cache. */ 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 6d157fb252..f7df72487a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,6 +25,7 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -146,8 +147,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException setCalibration( jsonWidth, jsonHeight, - Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), - Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics), + MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion)); setCalibError(jsonAvgError, jsonErrorStdDev); success = true; } @@ -184,7 +185,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); // create camera intrinsics matrix - var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1); setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); } @@ -597,17 +598,19 @@ public static SimCameraProperties PI4_LIFECAM_320_240() { prop.setCalibration( 320, 240, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 328.2733242048587, - 0.0, - 164.8190261141906, - 0.0, - 318.0609794305216, - 123.8633838438093, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.09957946553445934, -0.9166265114485799, @@ -626,17 +629,19 @@ public static SimCameraProperties PI4_LIFECAM_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 669.1428078983059, - 0.0, - 322.53377249329213, - 0.0, - 646.9843137061716, - 241.26567383784163, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.12788470750464645, -1.2350335805796528, @@ -655,17 +660,18 @@ public static SimCameraProperties LL2_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 511.22843367007755, - 0.0, - 323.62049380211096, - 0.0, - 514.5452336723849, - 261.8827920543568, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.1917469998873756, -0.5142936883324216, @@ -684,17 +690,19 @@ public static SimCameraProperties LL2_960_720() { prop.setCalibration( 960, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 769.6873145148892, - 0.0, - 486.1096609458122, - 0.0, - 773.8164483705323, - 384.66071662358354, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.189462064814501, -0.49903003669627627, @@ -713,17 +721,19 @@ public static SimCameraProperties LL2_1280_720() { prop.setCalibration( 1280, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 1011.3749416937393, - 0.0, - 645.4955139388737, - 0.0, - 1008.5391755084075, - 508.32877656020196, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.13730101577061535, -0.2904345656989261, diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 95eecb444c..02d9b46527 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -24,8 +24,11 @@ #include "photon/PhotonCamera.h" +#include + #include #include +#include #include #include "PhotonVersion.h" @@ -68,7 +71,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, rootTable->GetBooleanTopic("driverModeRequest").Publish()), m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}), path(rootTable->GetPath()), - m_cameraName(cameraName) {} + m_cameraName(cameraName) { + HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; +} PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} @@ -128,8 +134,13 @@ LEDMode PhotonCamera::GetLEDMode() const { std::optional PhotonCamera::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { - // clone should deal with ownership concerns? not sure - return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone(); + cv::Mat retVal(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + retVal.at(i, j) = camCoeffs[(j * 3) + i]; + } + } + return retVal; } return std::nullopt; } @@ -145,8 +156,11 @@ const std::string_view PhotonCamera::GetCameraName() const { std::optional PhotonCamera::GetDistCoeffs() { auto distCoeffs = cameraDistortionSubscriber.Get(); if (distCoeffs.size() == 5) { - // clone should deal with ownership concerns? not sure - return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone(); + cv::Mat retVal(5, 1, CV_64FC1); + for (int i = 0; i < 5; i++) { + retVal.at(i, 0) = distCoeffs[i]; + } + return retVal; } return std::nullopt; } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f2ff30f7ea..30bc6de2df 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -24,6 +24,8 @@ #include "photon/PhotonPoseEstimator.h" +#include + #include #include #include @@ -68,7 +70,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, PoseStrategy strat, PhotonCamera&& cam, @@ -79,7 +85,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 2a1418f1b5..99aa52c61b 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -39,11 +39,7 @@ #include #include -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting//PhotonPipelineResult.h" namespace cv { class Mat; @@ -172,6 +168,8 @@ class PhotonCamera { PhotonCamera::VERSION_CHECK_ENABLED = enabled; } + std::shared_ptr GetCameraTable() const { return rootTable; } + // For use in tests bool test = false; PhotonPipelineResult testResult; @@ -207,6 +205,7 @@ class PhotonCamera { private: units::second_t lastVersionCheckTime = 0_s; inline static bool VERSION_CHECK_ENABLED = true; + inline static int InstanceCount = 0; void VerifyVersion(); }; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index cdb60f6cee..2c863330bb 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -231,6 +231,8 @@ class PhotonPoseEstimator { units::second_t poseCacheTimestamp; + inline static int InstanceCount = 0; + inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } std::optional Update( diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h new file mode 100644 index 0000000000..367b30bb8f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -0,0 +1,449 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace photon { +class PhotonCameraSim { + public: + explicit PhotonCameraSim(PhotonCamera* camera) + : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {} + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props) + : prop(props), cam(camera) { + SetMinTargetAreaPixels(kDefaultMinAreaPx); + videoSimRaw = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), + prop.GetResHeight()); + videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray); + videoSimProcessed = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), + prop.GetResHeight()); + ts.subTable = cam->GetCameraTable(); + ts.UpdateEntries(); + } + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, + double minTargetAreaPercent, units::meter_t maxSightRange) + : PhotonCameraSim(camera, props) { + this->minTargetAreaPercent = minTargetAreaPercent; + this->maxSightRange = maxSightRange; + } + PhotonCamera* GetCamera() { return cam; } + double GetMinTargetAreaPercent() { return minTargetAreaPercent; } + double GetMinTargetAreaPixels() { + return minTargetAreaPercent / 100.0 * prop.GetResArea(); + } + units::meter_t GetMaxSightRange() { return maxSightRange; } + const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } + const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; } + bool CanSeeTargetPose(const frc::Pose3d& camPose, + const VisionTargetSim& target) { + CameraTargetRelation rel{camPose, target.GetPose()}; + return ((units::math::abs(rel.camToTargYaw.Degrees()) < + prop.GetHorizFOV().Degrees() / 2.0) && + (units::math::abs(rel.camToTargPitch.Degrees()) < + prop.GetVertFOV().Degrees() / 2.0) && + (!target.GetModel().GetIsPlanar() || + units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && + (rel.camToTarg.Translation().Norm() <= maxSightRange)); + } + bool CanSeeCorner(const std::vector& points) { + for (const auto& pt : points) { + if (std::clamp(pt.x, 0, prop.GetResWidth()) != pt.x || + std::clamp(pt.y, 0, prop.GetResHeight()) != pt.y) { + return false; + } + } + return true; + } + std::optional ConsumeNextEntryTime() { + uint64_t now = wpi::Now(); + uint64_t timestamp{}; + int iter = 0; + while (now >= nextNTEntryTime) { + timestamp = nextNTEntryTime; + uint64_t frameTime = prop.EstSecUntilNextFrame() + .convert() + .to(); + nextNTEntryTime += frameTime; + + if (iter++ > 50) { + timestamp = now; + nextNTEntryTime = now + frameTime; + break; + } + } + + if (timestamp != 0) { + return timestamp; + } else { + return std::nullopt; + } + } + void SetMinTargetAreaPercent(double areaPercent) { + minTargetAreaPercent = areaPercent; + } + void SetMinTargetAreaPixels(double areaPx) { + minTargetAreaPercent = areaPx / prop.GetResArea() * 100; + } + void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; } + void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; } + void SetWireframeResolution(double resolution) { + videoSimWireframeResolution = resolution; + } + void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } + PhotonPipelineResult Process(units::second_t latency, + const frc::Pose3d& cameraPose, + std::vector targets) { + std::sort( + targets.begin(), targets.end(), + [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { + units::meter_t dist1 = + t1.GetPose().Translation().Distance(cameraPose.Translation()); + units::meter_t dist2 = + t2.GetPose().Translation().Distance(cameraPose.Translation()); + return dist1 > dist2; + }); + + std::vector>> + visibleTgts{}; + std::vector detectableTgts{}; + RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose); + + VideoSimUtil::UpdateVideoProp(videoSimRaw, prop); + VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop); + cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()}; + cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1); + blankFrame.assignTo(videoSimFrameRaw); + + for (const auto& tgt : targets) { + if (!CanSeeTargetPose(cameraPose, tgt)) { + continue; + } + + std::vector fieldCorners = tgt.GetFieldVertices(); + if (tgt.GetModel().GetIsSpherical()) { + TargetModel model = tgt.GetModel(); + fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose( + tgt.GetPose().Translation(), cameraPose.Translation())); + } + + std::vector imagePoints = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners); + if (tgt.GetModel().GetIsSpherical()) { + cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints); + int l = 0; + int t = 0; + int b = 0; + int r = 0; + for (int i = 0; i < 4; i++) { + if (imagePoints[i].x < imagePoints[l].x) { + l = i; + } + } + cv::Point2d lc = imagePoints[l]; + std::array angles{}; + t = (l + 1) % 4; + b = (l + 1) % 4; + for (int i = 0; i < 4; i++) { + if (i == l) { + continue; + } + cv::Point2d ic = imagePoints[i]; + angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x); + if (angles[i] >= angles[t]) { + t = i; + } + if (angles[i] <= angles[b]) { + b = i; + } + } + for (int i = 0; i < 4; i++) { + if (i != t && i != l && i != b) { + r = i; + } + } + cv::RotatedRect rect{ + cv::Point2d{center.x, center.y}, + cv::Size2d{imagePoints[r].x - lc.x, + imagePoints[b].y - imagePoints[t].y}, + units::radian_t{-angles[r]}.convert().to()}; + std::vector points{}; + rect.points(points); + + // Can't find an easier way to convert from Point2f to Point2d + imagePoints.clear(); + std::transform(points.begin(), points.end(), + std::back_inserter(imagePoints), + [](const cv::Point2f& p) { return (cv::Point2d)p; }); + } + + visibleTgts.emplace_back(std::make_pair(tgt, imagePoints)); + std::vector noisyTargetCorners = + prop.EstPixelNoise(imagePoints); + cv::RotatedRect minAreaRect = + OpenCVHelp::GetMinAreaRect(noisyTargetCorners); + std::vector minAreaRectPts; + minAreaRectPts.reserve(4); + minAreaRect.points(minAreaRectPts); + cv::Point2d centerPt = minAreaRect.center; + frc::Rotation3d centerRot = prop.GetPixelRot(centerPt); + double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners); + + if (!(CanSeeCorner(noisyTargetCorners) && + areaPercent >= minTargetAreaPercent)) { + continue; + } + + PNPResult pnpSim{}; + if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { + pnpSim = OpenCVHelp::SolvePNP_Square( + prop.GetIntrinsics(), prop.GetDistCoeffs(), + tgt.GetModel().GetVertices(), noisyTargetCorners); + } + + std::vector> tempCorners = + OpenCVHelp::PointsToCorners(minAreaRectPts); + wpi::SmallVector, 4> smallVec; + + for (const auto& corner : tempCorners) { + smallVec.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + std::vector> cornersFloat = + OpenCVHelp::PointsToCorners(noisyTargetCorners); + + std::vector> cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; + detectableTgts.emplace_back(PhotonTrackedTarget{ + centerRot.Z().convert().to(), + -centerRot.Y().convert().to(), areaPercent, + centerRot.X().convert().to(), tgt.fiducialId, + pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble}); + } + + if (videoSimRawEnabled) { + if (videoSimWireframeEnabled) { + VideoSimUtil::DrawFieldWireFrame( + camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1, + cv::Scalar{30}, videoSimFrameRaw); + } + + for (const auto& pair : visibleTgts) { + VisionTargetSim tgt = pair.first; + std::vector corners = pair.second; + + if (tgt.fiducialId > 0) { + VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true, + videoSimFrameRaw); + } else if (!tgt.GetModel().GetIsSpherical()) { + std::vector contour = corners; + if (!tgt.GetModel().GetIsPlanar()) { + contour = OpenCVHelp::GetConvexHull(contour); + } + VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true, + videoSimFrameRaw); + } else { + VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255}, + videoSimFrameRaw); + } + } + videoSimRaw.PutFrame(videoSimFrameRaw); + } else { + videoSimRaw.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + if (videoSimProcEnabled) { + cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, + cv::COLOR_GRAY2BGR); + cv::drawMarker( + videoSimFrameProcessed, + cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0}, + cv::Scalar{0, 255, 0}, cv::MARKER_CROSS, + static_cast( + VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::LINE_AA); + for (const auto& tgt : detectableTgts) { + auto detectedCornersDouble = tgt.GetDetectedCorners(); + std::vector> detectedCornerFloat{ + detectedCornersDouble.begin(), detectedCornersDouble.end()}; + if (tgt.GetFiducialId() >= 0) { + VideoSimUtil::DrawTagDetection( + tgt.GetFiducialId(), + OpenCVHelp::CornersToPoints(detectedCornerFloat), + videoSimFrameProcessed); + } else { + cv::rectangle(videoSimFrameProcessed, + OpenCVHelp::GetBoundingRect( + OpenCVHelp::CornersToPoints(detectedCornerFloat)), + cv::Scalar{0, 0, 255}, + static_cast(VideoSimUtil::GetScaledThickness( + 1, videoSimFrameProcessed)), + cv::LINE_AA); + + wpi::SmallVector, 4> smallVec = + tgt.GetMinAreaRectCorners(); + + std::vector> cornersCopy{}; + cornersCopy.reserve(4); + + for (const auto& corner : smallVec) { + cornersCopy.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + VideoSimUtil::DrawPoly( + OpenCVHelp::CornersToPoints(cornersCopy), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed); + } + } + videoSimProcessed.PutFrame(videoSimFrameProcessed); + } else { + videoSimProcessed.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + MultiTargetPNPResult multiTagResults{}; + + std::vector visibleLayoutTags = + VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); + if (visibleLayoutTags.size() > 1) { + wpi::SmallVector usedIds{}; + std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), + usedIds.begin(), + [](const frc::AprilTag& tag) { return tag.ID; }); + std::sort(usedIds.begin(), usedIds.end()); + PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP( + prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, + kAprilTag16h5); + multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; + } + + return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + } + void SubmitProcessedFrame(const PhotonPipelineResult& result) { + SubmitProcessedFrame(result, wpi::Now()); + } + void SubmitProcessedFrame(const PhotonPipelineResult& result, + uint64_t recieveTimestamp) { + ts.latencyMillisEntry.Set( + result.GetLatency().convert().to(), + recieveTimestamp); + + Packet newPacket{}; + newPacket << result; + ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); + + bool hasTargets = result.HasTargets(); + ts.hasTargetEntry.Set(hasTargets, recieveTimestamp); + if (!hasTargets) { + ts.targetPitchEntry.Set(0.0, recieveTimestamp); + ts.targetYawEntry.Set(0.0, recieveTimestamp); + ts.targetAreaEntry.Set(0.0, recieveTimestamp); + std::array poseData{0.0, 0.0, 0.0}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetSkewEntry.Set(0.0, recieveTimestamp); + } else { + PhotonTrackedTarget bestTarget = result.GetBestTarget(); + + ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp); + ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp); + ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp); + ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp); + + frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); + std::array poseData{ + transform.X().to(), transform.Y().to(), + transform.Rotation().ToRotation2d().Degrees().to()}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + } + + auto intrinsics = prop.GetIntrinsics(); + std::vector intrinsicsView{intrinsics.data(), + intrinsics.data() + intrinsics.size()}; + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + + auto distortion = prop.GetDistCoeffs(); + std::vector distortionView{distortion.data(), + distortion.data() + distortion.size()}; + ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp); + + ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp); + } + SimCameraProperties prop; + + private: + PhotonCamera* cam; + + NTTopicSet ts{}; + uint64_t heartbeatCounter{0}; + + uint64_t nextNTEntryTime{wpi::Now()}; + + units::meter_t maxSightRange{std::numeric_limits::max()}; + static constexpr double kDefaultMinAreaPx{100}; + double minTargetAreaPercent; + + frc::AprilTagFieldLayout tagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + + cs::CvSource videoSimRaw; + cv::Mat videoSimFrameRaw{}; + bool videoSimRawEnabled{true}; + bool videoSimWireframeEnabled{false}; + double videoSimWireframeResolution{0.1}; + cs::CvSource videoSimProcessed; + cv::Mat videoSimFrameProcessed{}; + bool videoSimProcEnabled{true}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h new file mode 100644 index 0000000000..f3a5d73677 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -0,0 +1,475 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace photon { +class SimCameraProperties { + public: + SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + SimCameraProperties(std::string path, int width, int height) {} + void SetCalibration(int width, int height, frc::Rotation2d fovDiag) { + if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) { + fovDiag = frc::Rotation2d{ + std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; + FRC_ReportError( + frc::err::Error, + "Requested invalid FOV! Clamping between (1, 179) degrees..."); + } + double resDiag = std::sqrt(width * width + height * height); + double diagRatio = units::math::tan(fovDiag.Radians() / 2.0); + frc::Rotation2d fovWidth{ + units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; + frc::Rotation2d fovHeight{ + units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; + + Eigen::Matrix newDistCoeffs; + newDistCoeffs << 0, 0, 0, 0, 0; + + double cx = width / 2.0 - 0.5; + double cy = height / 2.0 - 0.5; + + double fx = cx / units::math::tan(fovWidth.Radians() / 2.0); + double fy = cy / units::math::tan(fovHeight.Radians() / 2.0); + + Eigen::Matrix newCamIntrinsics; + newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; + SetCalibration(width, height, newCamIntrinsics, newDistCoeffs); + } + + void SetCalibration(int width, int height, + const Eigen::Matrix& newCamIntrinsics, + const Eigen::Matrix& newDistCoeffs) { + resWidth = width; + resHeight = height; + camIntrinsics = newCamIntrinsics; + distCoeffs = newDistCoeffs; + + std::array p{ + frc::Translation3d{ + 1_m, + frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(0) + frc::Rotation2d{units::radian_t{ + -std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(width) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(0) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(height) + + frc::Rotation2d{ + units::radian_t{-std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + }; + viewplanes.clear(); + for (size_t i = 0; i < p.size(); i++) { + viewplanes.emplace_back(Eigen::Matrix{ + p[i].X().to(), p[i].Y().to(), p[i].Z().to()}); + } + } + + void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) { + avgErrorPx = newAvgErrorPx; + errorStdDevPx = newErrorStdDevPx; + } + + void SetFPS(units::hertz_t fps) { + frameSpeed = units::math::max(1 / fps, exposureTime); + } + + void SetExposureTime(units::second_t newExposureTime) { + exposureTime = newExposureTime; + frameSpeed = units::math::max(frameSpeed, exposureTime); + } + + void SetAvgLatency(units::second_t newAvgLatency) { + avgLatency = newAvgLatency; + } + + void SetLatencyStdDev(units::second_t newLatencyStdDev) { + latencyStdDev = newLatencyStdDev; + } + + int GetResWidth() const { return resWidth; } + + int GetResHeight() const { return resHeight; } + + int GetResArea() const { return resWidth * resHeight; } + + double GetAspectRatio() const { + return static_cast(resWidth) / static_cast(resHeight); + } + + Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } + + Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + + units::hertz_t GetFPS() const { return 1 / frameSpeed; } + + units::second_t GetFrameSpeed() const { return frameSpeed; } + + units::second_t GetExposureTime() const { return exposureTime; } + + units::second_t GetAverageLatency() const { return avgLatency; } + + units::second_t GetLatencyStdDev() const { return latencyStdDev; } + + double GetContourAreaPercent(const std::vector& points) { + return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) / + GetResArea() * 100; + } + + frc::Rotation2d GetPixelYaw(double pixelX) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - pixelX; + return frc::Rotation2d{fx, xOffset}; + } + + frc::Rotation2d GetPixelPitch(double pixelY) const { + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - pixelY; + return frc::Rotation2d{fy, -yOffset}; + } + + frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { + return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), + GetPixelYaw(point.x).Radians()}; + } + + frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - point.x; + + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - point.y; + + frc::Rotation2d yaw{fx, xOffset}; + frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset}; + return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; + } + + frc::Rotation2d GetHorizFOV() const { + frc::Rotation2d left = GetPixelYaw(0); + frc::Rotation2d right = GetPixelYaw(resWidth); + return left - right; + } + + frc::Rotation2d GetVertFOV() const { + frc::Rotation2d above = GetPixelPitch(0); + frc::Rotation2d below = GetPixelPitch(resHeight); + return below - above; + } + + frc::Rotation2d GetDiagFOV() const { + return frc::Rotation2d{ + units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + } + + std::pair, std::optional> GetVisibleLine( + const RotTrlTransform3d& camRt, const frc::Translation3d& a, + const frc::Translation3d& b) const { + frc::Translation3d relA = camRt.Apply(a); + frc::Translation3d relB = camRt.Apply(b); + + if (relA.X() <= 0_m && relB.X() <= 0_m) { + return {std::nullopt, std::nullopt}; + } + + Eigen::Matrix av{relA.X().to(), relA.Y().to(), + relA.Z().to()}; + Eigen::Matrix bv{relB.X().to(), relB.Y().to(), + relB.Z().to()}; + Eigen::Matrix abv = bv - av; + + bool aVisible = true; + bool bVisible = true; + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + double aVisibility = av.dot(normal); + if (aVisibility < 0) { + aVisible = false; + } + double bVisibility = bv.dot(normal); + if (bVisibility < 0) { + bVisible = false; + } + if (aVisibility <= 0 && bVisibility <= 0) { + return {std::nullopt, std::nullopt}; + } + } + + if (aVisible && bVisible) { + return {0, 1}; + } + + std::array intersections{std::nan(""), std::nan(""), + std::nan(""), std::nan("")}; + std::vector>> ipts{ + {std::nullopt, std::nullopt, std::nullopt, std::nullopt}}; + + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + Eigen::Matrix a_projn{}; + a_projn = (av.dot(normal) / normal.dot(normal)) * normal; + + if (std::abs(abv.dot(normal)) < 1e-5) { + continue; + } + intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn)); + + Eigen::Matrix apv{}; + apv = intersections[i] * abv; + Eigen::Matrix intersectpt{}; + intersectpt = av + apv; + ipts[i] = intersectpt; + + for (size_t j = 1; j < viewplanes.size(); j++) { + int oi = (i + j) % viewplanes.size(); + Eigen::Matrix onormal = viewplanes[oi]; + if (intersectpt.dot(onormal) < 0) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + + if (!ipts[i]) { + continue; + } + + for (int j = i - 1; j >= 0; j--) { + std::optional> oipt = ipts[j]; + if (!oipt) { + continue; + } + Eigen::Matrix diff{}; + diff = oipt.value() - intersectpt; + if (diff.cwiseAbs().maxCoeff() < 1e-4) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + } + + double inter1 = std::nan(""); + double inter2 = std::nan(""); + for (double inter : intersections) { + if (!std::isnan(inter)) { + if (std::isnan(inter1)) { + inter1 = inter; + } else { + inter2 = inter; + } + } + } + + if (!std::isnan(inter2)) { + double max = std::max(inter1, inter2); + double min = std::min(inter1, inter2); + if (aVisible) { + min = 0; + } + if (bVisible) { + max = 1; + } + return {min, max}; + } else if (!std::isnan(inter1)) { + if (aVisible) { + return {0, inter1}; + } + if (bVisible) { + return {inter1, 1}; + } + return {inter1, std::nullopt}; + } else { + return {std::nullopt, std::nullopt}; + } + } + + std::vector EstPixelNoise( + const std::vector& points) { + if (avgErrorPx == 0 && errorStdDevPx == 0) { + return points; + } + + std::vector noisyPts; + noisyPts.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + cv::Point2f p = points[i]; + float error = avgErrorPx + gaussian(generator) * errorStdDevPx; + float errorAngle = + uniform(generator) * 2 * std::numbers::pi - std::numbers::pi; + noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle), + p.y + error * std::sin(errorAngle)}); + } + return noisyPts; + } + + units::second_t EstLatency() { + return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, + 0_s); + } + + units::second_t EstSecUntilNextFrame() { + return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); + } + + static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; } + + static SimCameraProperties PI4_LIFECAM_320_240() { + SimCameraProperties prop{}; + prop.SetCalibration( + 320, 240, + (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906, + 0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.09957946553445934, -0.9166265114485799, 0.0019519890627236526, + -0.0036071725380870333, 1.5627234622420942}); + prop.SetCalibError(0.21, 0.0124); + prop.SetFPS(30_Hz); + prop.SetAvgLatency(30_ms); + prop.SetLatencyStdDev(10_ms); + return prop; + } + + static SimCameraProperties PI4_LIFECAM_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213, + 0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.12788470750464645, -1.2350335805796528, 0.0024990767286192732, + -0.0026958287600230705, 2.2951386729115537}); + prop.SetCalibError(0.26, 0.046); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(65_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096, + 0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.1917469998873756, -0.5142936883324216, + 0.012461562046896614, 0.0014084973492408186, + 0.35160648971214437}); + prop.SetCalibError(0.25, 0.05); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(35_ms); + prop.SetLatencyStdDev(8_ms); + return prop; + } + + static SimCameraProperties LL2_960_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 960, 720, + (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122, + 0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.189462064814501, -0.49903003669627627, + 0.007468423590519429, 0.002496885298683693, + 0.3443122090208624}); + prop.SetCalibError(0.35, 0.10); + prop.SetFPS(10_Hz); + prop.SetAvgLatency(50_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_1280_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 1280, 720, + (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737, + 0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.13730101577061535, -0.2904345656989261, + 8.32475714507539E-4, -3.694397782014239E-4, + 0.09487962227027584}); + prop.SetCalibError(0.37, 0.06); + prop.SetFPS(7_Hz); + prop.SetAvgLatency(60_ms); + prop.SetLatencyStdDev(20_ms); + return prop; + } + + private: + std::mt19937 generator{std::random_device{}()}; + std::normal_distribution gaussian{0.0, 1.0}; + std::uniform_real_distribution uniform{0.0, 1.0}; + + int resWidth; + int resHeight; + Eigen::Matrix camIntrinsics; + Eigen::Matrix distCoeffs; + double avgErrorPx; + double errorStdDevPx; + units::second_t frameSpeed{0}; + units::second_t exposureTime{0}; + units::second_t avgLatency{0}; + units::second_t latencyStdDev{0}; + std::vector> viewplanes{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h new file mode 100644 index 0000000000..bb2c453a88 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -0,0 +1,432 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "SimCameraProperties.h" +#include "photon/estimation/RotTrlTransform3d.h" + +namespace mathutil { +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} +} // namespace mathutil + +namespace photon { +namespace VideoSimUtil { +static constexpr int kNumTags16h5 = 30; + +static constexpr units::meter_t fieldLength{16.54175_m}; +static constexpr units::meter_t fieldWidth{8.0137_m}; + +static cv::Mat Get16h5TagImage(int id) { + wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id); + cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data}; + cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone(); + return markerClone; +} + +static std::unordered_map LoadAprilTagImages() { + std::unordered_map retVal{}; + for (int i = 0; i < kNumTags16h5; i++) { + cv::Mat tagImage = Get16h5TagImage(i); + retVal[i] = tagImage; + } + return retVal; +} + +static std::vector GetImageCorners(const cv::Size& size) { + std::vector retVal{}; + retVal.emplace_back(cv::Point2f{-0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f}); + retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f}); + return retVal; +} + +static std::vector Get16h5MarkerPts(int scale) { + cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}}; + roi16h5.x *= scale; + roi16h5.y *= scale; + roi16h5.width *= scale; + roi16h5.height *= scale; + std::vector pts = GetImageCorners(roi16h5.size()); + for (size_t i = 0; i < pts.size(); i++) { + cv::Point2f pt = pts[i]; + pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y}; + } + return pts; +} + +static std::vector Get16h5MarkerPts() { + return Get16h5MarkerPts(1); +} + +static const std::unordered_map kTag16h5Images = + LoadAprilTagImages(); +static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); + +[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, + const SimCameraProperties& prop) { + video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); + video.SetFPS(prop.GetFPS().to()); +} + +[[maybe_unused]] static void Warp165h5TagImage( + int tagId, const std::vector& dstPoints, bool antialiasing, + cv::Mat& destination) { + if (!kTag16h5Images.contains(tagId)) { + return; + } + cv::Mat tagImage = kTag16h5Images.at(tagId); + std::vector tagPoints{kTag16h5MarkPts}; + std::vector tagImageCorners{GetImageCorners(tagImage.size())}; + std::vector dstPointMat = dstPoints; + cv::Rect boundingRect = cv::boundingRect(dstPointMat); + cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat); + std::vector extremeCorners{}; + cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf); + boundingRect = cv::boundingRect(extremeCorners); + + double warpedContourArea = cv::contourArea(extremeCorners); + double warpedTagUpscale = + std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area()); + int warpStrat = cv::INTER_NEAREST; + + int supersampling = 6; + supersampling = static_cast(std::ceil(supersampling / warpedTagUpscale)); + supersampling = std::max(std::min(supersampling, 8), 1); + + cv::Mat scaledTagImage{}; + if (warpedTagUpscale > 2.0) { + warpStrat = cv::INTER_LINEAR; + int scaleFactor = static_cast(warpedTagUpscale / 3.0) + 2; + scaleFactor = std::max(std::min(scaleFactor, 40), 1); + scaleFactor *= supersampling; + cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor, + cv::INTER_NEAREST); + tagPoints = Get16h5MarkerPts(scaleFactor); + } else { + scaledTagImage = tagImage; + } + + boundingRect.x -= 1; + boundingRect.y -= 1; + boundingRect.width += 2; + boundingRect.height += 2; + if (boundingRect.x < 0) { + boundingRect.width += boundingRect.x; + boundingRect.x = 0; + } + if (boundingRect.y < 0) { + boundingRect.height += boundingRect.y; + boundingRect.y = 0; + } + boundingRect.width = + std::min(destination.size().width - boundingRect.x, boundingRect.width); + boundingRect.height = + std::min(destination.size().height - boundingRect.y, boundingRect.height); + if (boundingRect.width <= 0 || boundingRect.height <= 0) { + return; + } + + std::vector scaledDstPts{}; + if (supersampling > 1) { + cv::multiply(dstPointMat, + cv::Scalar{static_cast(supersampling), + static_cast(supersampling)}, + scaledDstPts); + boundingRect.x *= supersampling; + boundingRect.y *= supersampling; + boundingRect.width *= supersampling; + boundingRect.height *= supersampling; + } else { + scaledDstPts = dstPointMat; + } + + cv::subtract(scaledDstPts, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + scaledDstPts); + perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts); + + cv::Mat tempRoi{}; + cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(), + warpStrat); + + if (supersampling > 1) { + boundingRect.x /= supersampling; + boundingRect.y /= supersampling; + boundingRect.width /= supersampling; + boundingRect.height /= supersampling; + cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA); + } + + cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)}; + cv::subtract(extremeCorners, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + extremeCorners); + cv::Point2f tempCenter{}; + tempCenter.x = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.x / extremeCorners.size(); + }); + tempCenter.y = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.y / extremeCorners.size(); + }); + + for (auto& corner : extremeCorners) { + float xDiff = corner.x - tempCenter.x; + float yDiff = corner.y - tempCenter.y; + xDiff += 1 * mathutil::sgn(xDiff); + yDiff += 1 * mathutil::sgn(yDiff); + corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff}; + } + + std::vector extremeCornerInt{extremeCorners.begin(), + extremeCorners.end()}; + cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255}); + + cv::copyTo(tempRoi, destination(boundingRect), tempMask); +} + +static double GetScaledThickness(double thickness480p, + const cv::Mat& destinationMat) { + double scaleX = destinationMat.size().width / 640.0; + double scaleY = destinationMat.size().height / 480.0; + double minScale = std::min(scaleX, scaleY); + return std::max(thickness480p * minScale, 1.0); +} + +[[maybe_unused]] static void DrawInscribedEllipse( + const std::vector& dstPoints, const cv::Scalar& color, + cv::Mat& destination) { + cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints); + cv::ellipse(destination, rect, color, -1, cv::LINE_AA); +} + +static void DrawPoly(const std::vector& dstPoints, int thickness, + const cv::Scalar& color, bool isClosed, + cv::Mat& destination) { + std::vector intDstPoints{dstPoints.begin(), dstPoints.end()}; + std::vector> listOfListOfPoints; + listOfListOfPoints.emplace_back(intDstPoints); + if (thickness > 0) { + cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness, + cv::LINE_AA); + } else { + cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA); + } +} + +[[maybe_unused]] static void DrawTagDetection( + int id, const std::vector& dstPoints, cv::Mat& destination) { + double thickness = GetScaledThickness(1, destination); + DrawPoly(dstPoints, static_cast(thickness), cv::Scalar{0, 0, 255}, true, + destination); + cv::Rect2d rect{cv::boundingRect(dstPoints)}; + cv::Point2d textPt{rect.x + rect.width, rect.y}; + textPt.x += thickness; + textPt.y += thickness; + cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN, + 1.5 * thickness, cv::Scalar{0, 200, 0}, + static_cast(thickness), cv::LINE_AA); +} + +static std::vector> GetFieldWallLines() { + std::vector> list; + + const units::meter_t sideHt = 19.5_in; + const units::meter_t driveHt = 35_in; + const units::meter_t topHt = 78_in; + + // field floor + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, + frc::Translation3d{fieldLength, 0_m, 0_m}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, 0_m, 0_m}}); + + // right side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, 0_m}}); + + // red driverstation + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, topHt}, + frc::Translation3d{fieldLength, fieldWidth, topHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, driveHt}, + frc::Translation3d{fieldLength, fieldWidth, driveHt}}); + + // left side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}}); + + // blue driverstation + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{0_m, 0_m, topHt}, + frc::Translation3d{0_m, fieldWidth, topHt}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, driveHt}, + frc::Translation3d{0_m, fieldWidth, driveHt}}); + + return list; +} + +static std::vector> GetFieldFloorLines( + int subdivisions) { + std::vector> list; + const units::meter_t subLength = fieldLength / subdivisions; + const units::meter_t subWidth = fieldWidth / subdivisions; + + for (int i = 0; i < subdivisions; i++) { + list.emplace_back(std::vector{ + frc::Translation3d{0_m, subWidth * (i + 1), 0_m}, + frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); + list.emplace_back(std::vector{ + frc::Translation3d{subLength * (i + 1), 0_m, 0_m}, + frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); + } + return list; +} + +static std::vector> PolyFrom3dLines( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + const std::vector& trls, double resolution, + bool isClosed, cv::Mat& destination) { + resolution = std::hypot(destination.size().height, destination.size().width) * + resolution; + std::vector pts{trls}; + if (isClosed) { + pts.emplace_back(pts[0]); + } + std::vector> polyPointList{}; + + for (size_t i = 0; i < pts.size() - 1; i++) { + frc::Translation3d pta = pts[i]; + frc::Translation3d ptb = pts[i + 1]; + + std::pair, std::optional> inter = + prop.GetVisibleLine(camRt, pta, ptb); + if (!inter.second) { + continue; + } + + double inter1 = inter.first.value(); + double inter2 = inter.second.value(); + frc::Translation3d baseDelta = ptb - pta; + frc::Translation3d old_pta = pta; + if (inter1 > 0) { + pta = old_pta + baseDelta * inter1; + } + if (inter2 < 1) { + ptb = old_pta + baseDelta * inter2; + } + baseDelta = ptb - pta; + + std::vector poly = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb}); + cv::Point2d pxa = poly[0]; + cv::Point2d pxb = poly[1]; + + double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y); + int subdivisions = static_cast(pxDist / resolution); + frc::Translation3d subDelta = baseDelta / (subdivisions + 1); + std::vector subPts{}; + for (int j = 0; j < subdivisions; j++) { + subPts.emplace_back(pta + (subDelta * (j + 1))); + } + if (subPts.size() > 0) { + std::vector toAdd = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts); + poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end()); + } + + polyPointList.emplace_back(poly); + } + + return polyPointList; +} + +[[maybe_unused]] static void DrawFieldWireFrame( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + double resolution, double wallThickness, const cv::Scalar& wallColor, + int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor, + cv::Mat& destination) { + for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(floorThickness, destination))), + floorColor, false, destination); + } + } + for (const auto& trls : GetFieldWallLines()) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(wallThickness, destination))), + wallColor, false, destination); + } + } +} +} // namespace VideoSimUtil +} // 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 new file mode 100644 index 0000000000..55988bc32f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -0,0 +1,282 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "photon/simulation/PhotonCameraSim.h" + +namespace photon { +class VisionSystemSim { + public: + explicit VisionSystemSim(std::string visionSystemName) { + std::string tableName = "VisionSystemSim-" + visionSystemName; + frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + } + std::optional GetCameraSim(std::string name) { + auto it = camSimMap.find(name); + if (it != camSimMap.end()) { + return std::make_optional(it->second); + } else { + return std::nullopt; + } + } + std::vector GetCameraSims() { + std::vector retVal; + for (auto const& cam : camSimMap) { + retVal.emplace_back(cam.second); + } + return retVal; + } + void AddCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + auto found = + camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (found == camSimMap.end()) { + camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] = + cameraSim; + camTrfMap.insert(std::make_pair( + std::move(cameraSim), + frc::TimeInterpolatableBuffer{bufferLength})); + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + } + } + void ClearCameras() { + camSimMap.clear(); + camTrfMap.clear(); + } + bool RemoveCamera(PhotonCameraSim* cameraSim) { + int numOfElementsRemoved = + camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (numOfElementsRemoved == 1) { + return true; + } else { + return false; + } + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { + return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, + units::second_t time) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + frc::TimeInterpolatableBuffer trfBuffer = + camTrfMap.at(cameraSim); + std::optional sample = trfBuffer.Sample(time); + if (!sample) { + return std::nullopt; + } else { + return std::make_optional( + frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})}); + } + } else { + return std::nullopt; + } + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim) { + return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim, + units::second_t time) { + auto robotToCamera = GetRobotToCamera(cameraSim, time); + if (!robotToCamera) { + return std::nullopt; + } else { + return std::make_optional(GetRobotPose(time) + robotToCamera.value()); + } + } + bool AdjustCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + return true; + } else { + return false; + } + } + void ResetCameraTransforms() { + for (const auto& pair : camTrfMap) { + ResetCameraTransforms(pair.first); + } + } + bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { + units::second_t now = frc::Timer::GetFPGATimestamp(); + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + auto trfBuffer = camTrfMap.at(cameraSim); + frc::Transform3d lastTrf{frc::Pose3d{}, + trfBuffer.Sample(now).value_or(frc::Pose3d{})}; + trfBuffer.Clear(); + AdjustCamera(cameraSim, lastTrf); + return true; + } else { + return false; + } + } + std::vector GetVisionTargets() { + std::vector all{}; + for (const auto& entry : targetSets) { + for (const auto& target : entry.second) { + all.emplace_back(target); + } + } + return all; + } + std::vector GetVisionTargets(std::string type) { + return targetSets[type]; + } + void AddVisionTargets(const std::vector& targets) { + AddVisionTargets("targets", targets); + } + void AddVisionTargets(std::string type, + const std::vector& targets) { + if (!targetSets.contains(type)) { + targetSets[type] = std::vector{}; + } + for (const auto& tgt : targets) { + targetSets[type].emplace_back(tgt); + } + } + void AddAprilTags(const frc::AprilTagFieldLayout& layout) { + std::vector targets; + for (const frc::AprilTag& tag : layout.GetTags()) { + targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), + photon::kAprilTag16h5, tag.ID}); + } + AddVisionTargets("apriltag", targets); + } + void ClearVisionTargets() { targetSets.clear(); } + void ClearAprilTags() { RemoveVisionTargets("apriltag"); } + void RemoveVisionTargets(std::string type) { targetSets.erase(type); } + std::vector RemoveVisionTargets( + const std::vector& targets) { + std::vector removedList; + for (auto& entry : targetSets) { + for (auto target : entry.second) { + auto it = std::find(targets.begin(), targets.end(), target); + if (it != targets.end()) { + removedList.emplace_back(target); + entry.second.erase(it); + } + } + } + return removedList; + } + frc::Pose3d GetRobotPose() { + return GetRobotPose(frc::Timer::GetFPGATimestamp()); + } + frc::Pose3d GetRobotPose(units::second_t timestamp) { + return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); + } + void ResetRobotPose(const frc::Pose2d& robotPose) { + ResetRobotPose(frc::Pose3d{robotPose}); + } + void ResetRobotPose(const frc::Pose3d& robotPose) { + robotPoseBuffer.Clear(); + robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); + } + frc::Field2d& GetDebugField() { return dbgField; } + void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + void Update(const frc::Pose3d& robotPose) { + for (auto& set : targetSets) { + std::vector posesToAdd{}; + for (auto& target : set.second) { + posesToAdd.emplace_back(target.GetPose().ToPose2d()); + } + dbgField.GetObject(set.first)->SetPoses(posesToAdd); + } + + units::second_t now = frc::Timer::GetFPGATimestamp(); + robotPoseBuffer.AddSample(now, robotPose); + dbgField.SetRobotPose(robotPose.ToPose2d()); + + std::vector allTargets{}; + for (const auto& set : targetSets) { + for (const auto& target : set.second) { + allTargets.emplace_back(target); + } + } + + std::vector visTgtPoses2d{}; + std::vector cameraPoses2d{}; + bool processed{false}; + for (const auto& entry : camSimMap) { + auto camSim = entry.second; + auto optTimestamp = camSim->ConsumeNextEntryTime(); + if (!optTimestamp) { + continue; + } else { + processed = true; + } + uint64_t timestampNt = optTimestamp.value(); + units::second_t latency = camSim->prop.EstLatency(); + units::second_t timestampCapture = + units::microsecond_t{static_cast(timestampNt)} - latency; + + frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture); + frc::Pose3d lateCameraPose = + lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); + cameraPoses2d.push_back(lateCameraPose.ToPose2d()); + + auto camResult = camSim->Process(latency, lateCameraPose, allTargets); + camSim->SubmitProcessedFrame(camResult, timestampNt); + for (const auto& target : camResult.GetTargets()) { + auto trf = target.GetBestCameraToTarget(); + if (trf == kEmptyTrf) { + continue; + } + visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d()); + } + } + if (processed) { + dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d); + } + if (cameraPoses2d.size() != 0) { + dbgField.GetObject("cameras")->SetPoses(cameraPoses2d); + } + } + + private: + std::unordered_map camSimMap{}; + static constexpr units::second_t bufferLength{1.5_s}; + std::unordered_map> + camTrfMap; + frc::TimeInterpolatableBuffer robotPoseBuffer{bufferLength}; + std::unordered_map> targetSets{}; + frc::Field2d dbgField{}; + const frc::Transform3d kEmptyTrf{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h new file mode 100644 index 0000000000..f8d4d6676b --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h @@ -0,0 +1,73 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include + +#include "photon/estimation/TargetModel.h" + +namespace photon { +class VisionTargetSim { + public: + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model) + : fiducialId(-1), pose(pose), model(model) {} + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id) + : fiducialId(id), pose(pose), model(model) {} + void SetPose(const frc::Pose3d& newPose) { pose = newPose; } + void SetModel(const TargetModel& newModel) { model = newModel; } + frc::Pose3d GetPose() const { return pose; } + TargetModel GetModel() const { return model; } + std::vector GetFieldVertices() const { + return model.GetFieldVertices(pose); + } + int fiducialId; + + bool operator<(const VisionTargetSim& right) const { + return pose.Translation().Norm() < right.pose.Translation().Norm(); + } + + bool operator==(const VisionTargetSim& other) const { + return units::math::abs(pose.Translation().X() - + other.GetPose().Translation().X()) < 1_in && + units::math::abs(pose.Translation().Y() - + other.GetPose().Translation().Y()) < 1_in && + units::math::abs(pose.Translation().Z() - + other.GetPose().Translation().Z()) < 1_in && + units::math::abs(pose.Rotation().X() - + other.GetPose().Rotation().X()) < 1_deg && + units::math::abs(pose.Rotation().Y() - + other.GetPose().Rotation().Y()) < 1_deg && + units::math::abs(pose.Rotation().Z() - + other.GetPose().Rotation().Z()) < 1_deg && + model.GetIsPlanar() == other.GetModel().GetIsPlanar(); + } + + private: + frc::Pose3d pose; + TargetModel model; +}; +} // namespace photon diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp new file mode 100644 index 0000000000..1c3f9eac58 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -0,0 +1,461 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "gtest/gtest.h" +#include "photon/PhotonUtils.h" +#include "photon/simulation/VisionSystemSim.h" + +class VisionSystemSimTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photon::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class VisionSystemSimTestWithParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface {}; +class VisionSystemSimTestDistanceParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + + // To the right, to the right + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + + frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, + frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + + frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(1.0); + cameraSim.SetMaxSightRange(10_m); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + + frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25); +} + +TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg)); + +TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { + units::foot_t distParam; + units::degree_t pitchParam; + units::foot_t heightParam; + std::tie(distParam, pitchParam, heightParam) = GetParam(); + + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; + frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; + frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, + frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + photon::VisionSystemSim visionSysSim{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AdjustCamera(&cameraSim, robotToCamera); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.Update(robotPose); + + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + photon::PhotonTrackedTarget target = res.GetBestTarget(); + + ASSERT_NEAR(0.0, target.GetYaw(), 0.5); + + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, VisionSystemSimTestDistanceParamsTest, + testing::Values(std::make_tuple(5_ft, -15.98_deg, 0_ft), + std::make_tuple(6_ft, -15.98_deg, 1_ft), + std::make_tuple(10_ft, -15.98_deg, 0_ft), + std::make_tuple(15_ft, -15.98_deg, 2_ft), + std::make_tuple(19.95_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -15.98_deg, 0_ft), + std::make_tuple(5_ft, -42_deg, 1_ft), + std::make_tuple(6_ft, -42_deg, 0_ft), + std::make_tuple(10_ft, -42_deg, 2_ft), + std::make_tuple(15_ft, -42_deg, 0.5_ft), + std::make_tuple(19.42_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -42_deg, 0_ft), + std::make_tuple(5_ft, -55_deg, 2_ft), + std::make_tuple(6_ft, -55_deg, 0_ft), + std::make_tuple(10_ft, -54_deg, 2.2_ft), + std::make_tuple(15_ft, -53_deg, 0_ft), + std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft))); + +TEST_F(VisionSystemSimTest, TestMultipleTargets) { + frc::Pose3d targetPoseL{ + frc::Translation3d{15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseC{ + frc::Translation3d{15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseR{ + frc::Translation3d{15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 2}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 3}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 4}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 5}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 6}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 7}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 8}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 9}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 10}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 11}}); + + frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + visionSysSim.Update(robotPose); + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + std::span tgtList = res.GetTargets(); + ASSERT_EQ(static_cast(11), tgtList.size()); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimation) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen; + cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen); + + Eigen::Matrix distEigen; + cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag16h5); + frc::Pose3d pose = frc::Pose3d{} + results.best; + ASSERT_NEAR(5, pose.X().to(), 0.01); + ASSERT_NEAR(1, pose.Y().to(), 0.01); + ASSERT_NEAR(0, pose.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); + frc::Pose3d pose2 = frc::Pose3d{} + results2.best; + ASSERT_NEAR(5, pose2.X().to(), 0.01); + ASSERT_NEAR(1, pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} diff --git a/photon-server/build.gradle b/photon-server/build.gradle index 2fdfcff4ec..c60c441760 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -6,32 +6,20 @@ plugins { id 'edu.wpi.first.WpilibTools' version '1.3.0' } -evaluationDependsOn(':photon-core') - -mainClassName = 'org.photonvision.Main' - -group 'org.photonvision' -version versionString + (project.hasProperty('pionly') ? "-raspi" : "") - apply from: "${rootDir}/shared/common.gradle" dependencies { implementation project(':photon-core') - implementation project(':photon-targeting') - implementation "io.javalin:javalin:$javalinVersion" + // Needed for Javalin Runtime Logging implementation "org.slf4j:slf4j-simple:2.0.7" +} - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) +group 'org.photonvision' +version versionString + (project.hasProperty('pionly') ? "-raspi" : "") - implementation "org.msgpack:msgpack-core:0.9.0" - implementation "org.msgpack:jackson-dataformat-msgpack:0.9.0" +application { + mainClass = 'org.photonvision.Main' } shadowJar { @@ -43,7 +31,6 @@ shadowJar { ] } - node { nodeProjectDir = file("${projectDir}/../photon-client") } @@ -58,6 +45,10 @@ tasks.register("buildAndCopyUI") { finalizedBy "copyClientUIToResources" } +run { + environment "PATH_PREFIX", "../" +} + run { if (project.hasProperty("profile")) { jvmArgs=[ @@ -70,7 +61,6 @@ run { } } - remotes { pi { host = 'photonvision.local' @@ -119,10 +109,6 @@ task findDeployTarget { } } -run { - environment "PATH_PREFIX", "../" -} - task deploy { dependsOn findDeployTarget dependsOn assemble diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index 81f669c4c2..d277f2904f 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -89,8 +89,9 @@ public static void onSettingsImportRequest(Context ctx) { if (ConfigManager.saveUploadedSettingsZip(tempFilePath.get())) { ctx.status(200); - ctx.result("Successfully saved the uploaded settings zip"); - logger.info("Successfully saved the uploaded settings zip"); + ctx.result("Successfully saved the uploaded settings zip, rebooting"); + logger.info("Successfully saved the uploaded settings zip, rebooting"); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded zip file"); diff --git a/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so b/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so index 90bb006943..c2d0d9b496 100644 Binary files a/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so and b/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so differ diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 6f91d82dc6..4238e951e6 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -6,45 +6,4 @@ ext { nativeName = "photontargeting" } -apply from: "${rootDir}/shared/setupBuild.gradle" - - -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() - -def nativeConfigName = 'wpilibNatives' -def nativeConfig = configurations.create(nativeConfigName) - -def nativeTasks = wpilibTools.createExtractionTasks { - configurationName = nativeConfigName -} - -nativeTasks.addToSourceSetResources(sourceSets.main) - -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") -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" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - -dependencies { - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("cameraserver") - implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - - implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() - implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() - - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); -} +apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ed53a17b80..e2413a4d22 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -18,6 +18,7 @@ package org.photonvision.estimation; import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -63,8 +64,8 @@ public static void forceLoadOpenCV() { } static { - NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); - EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); + NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 9a8ce65db9..7047ee46aa 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -88,7 +88,7 @@ public PhotonTrackedTarget getBestTarget() { String errStr = "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " + "before calling this method. For more information, please review the PhotonLib " - + "documentation at http://docs.photonvision.org"; + + "documentation at https://docs.photonvision.org"; System.err.println(errStr); new Exception().printStackTrace(); HAS_WARNED = true; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index dc847ec5fc..392a429006 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -97,8 +97,9 @@ public int getFiducialId() { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is betweeen 0 + * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers + * above 0.2 are likely to be ambiguous. -1 if invalid. */ public double getPoseAmbiguity() { return poseAmbiguity; diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h new file mode 100644 index 0000000000..a0a0fb0e61 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -0,0 +1,61 @@ +/* + * 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 . + */ + +#pragma once + +#include + +namespace photon { +class CameraTargetRelation { + public: + const frc::Pose3d camPose; + const frc::Transform3d camToTarg; + const units::meter_t camToTargDist; + const units::meter_t camToTargDistXY; + const frc::Rotation2d camToTargYaw; + const frc::Rotation2d camToTargPitch; + + const frc::Rotation2d camToTargAngle; + + const frc::Transform3d targToCam; + const frc::Rotation2d targToCamYaw; + const frc::Rotation2d targToCamPitch; + + const frc::Rotation2d targToCamAngle; + + CameraTargetRelation(const frc::Pose3d& cameraPose, + const frc::Pose3d& targetPose) + : camPose(cameraPose), + camToTarg(frc::Transform3d{cameraPose, targetPose}), + camToTargDist(camToTarg.Translation().Norm()), + camToTargDistXY(units::math::hypot(camToTarg.Translation().X(), + camToTarg.Translation().Y())), + camToTargYaw(frc::Rotation2d{camToTarg.X().to(), + camToTarg.Y().to()}), + camToTargPitch(frc::Rotation2d{camToTargDistXY.to(), + -camToTarg.Z().to()}), + camToTargAngle(frc::Rotation2d{units::math::hypot( + camToTargYaw.Radians(), camToTargPitch.Radians())}), + targToCam(frc::Transform3d{targetPose, cameraPose}), + targToCamYaw(frc::Rotation2d{targToCam.X().to(), + targToCam.Y().to()}), + targToCamPitch(frc::Rotation2d{camToTargDistXY.to(), + -targToCam.Z().to()}), + targToCamAngle(frc::Rotation2d{units::math::hypot( + targToCamYaw.Radians(), targToCamPitch.Radians())}) {} +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h new file mode 100644 index 0000000000..2d79e661e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -0,0 +1,291 @@ +/* + * 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 . + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "RotTrlTransform3d.h" + +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { +namespace OpenCVHelp { + +static frc::Rotation3d NWU_TO_EDN{ + (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()}; +static frc::Rotation3d EDN_TO_NWU{ + (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; + +static std::vector GetConvexHull( + const std::vector& points) { + std::vector outputHull{}; + cv::convexHull(points, outputHull); + std::vector convexPoints; + for (size_t i = 0; i < outputHull.size(); i++) { + convexPoints.push_back(points[outputHull[i]]); + } + return convexPoints; +} + +static cv::RotatedRect GetMinAreaRect(const std::vector& points) { + return cv::minAreaRect(points); +} + +static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) { + return trl.RotateBy(NWU_TO_EDN); +} + +static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) { + return -NWU_TO_EDN + (rot + NWU_TO_EDN); +} + +static std::vector TranslationToTVec( + const std::vector& translations) { + std::vector retVal; + retVal.reserve(translations.size()); + for (size_t i = 0; i < translations.size(); i++) { + frc::Translation3d trl = TranslationNWUtoEDN(translations[i]); + retVal.emplace_back(cv::Point3f{trl.X().to(), trl.Y().to(), + trl.Z().to()}); + } + return retVal; +} + +static std::vector RotationToRVec( + const frc::Rotation3d& rotation) { + std::vector retVal{}; + frc::Rotation3d rot = RotationNWUtoEDN(rotation); + retVal.emplace_back(cv::Point3d{ + rot.GetQuaternion().ToRotationVector()(0), + rot.GetQuaternion().ToRotationVector()(1), + rot.GetQuaternion().ToRotationVector()(2), + }); + return retVal; +} + +[[maybe_unused]] static cv::Point2f AvgPoint(std::vector points) { + if (points.size() == 0) { + return cv::Point2f{}; + } + cv::reduce(points, points, 0, cv::REDUCE_AVG); + return points[0]; +} + +[[maybe_unused]] static std::vector> PointsToCorners( + const std::vector& points) { + std::vector> retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(std::make_pair(points[i].x, points[i].y)); + } + return retVal; +} + +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector>& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second}); + } + return retVal; +} + +[[maybe_unused]] static cv::Rect GetBoundingRect( + const std::vector& points) { + return cv::boundingRect(points); +} + +static std::vector ProjectPoints( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const RotTrlTransform3d& camRt, + const std::vector& objectTranslations) { + std::vector objectPoints = TranslationToTVec(objectTranslations); + std::vector rvec = RotationToRVec(camRt.GetRotation()); + std::vector tvec = TranslationToTVec({camRt.GetTranslation()}); + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + std::vector imagePoints{}; + cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat, + imagePoints); + return imagePoints; +} + +template +static std::vector ReorderCircular(const std::vector elements, + bool backwards, int shiftStart) { + size_t size = elements.size(); + int dir = backwards ? -1 : 1; + std::vector reordered{elements}; + for (size_t i = 0; i < size; i++) { + int index = (i * dir + shiftStart * dir) % size; + if (index < 0) { + index = size + index; + } + reordered[i] = elements[index]; + } + return reordered; +} + +static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) { + return trl.RotateBy(EDN_TO_NWU); +} + +static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) { + return -EDN_TO_NWU + (rot + EDN_TO_NWU); +} + +static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; + tvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]}, + units::meter_t{data[1]}, + units::meter_t{data[2]}}); +} + +static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; + rvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]}, + units::radian_t{data[1]}, + units::radian_t{data[2]}}); +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_Square( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + modelTrls = ReorderCircular(modelTrls, true, -1); + imagePoints = ReorderCircular(imagePoints, true, -1); + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs; + std::vector tvecs; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + cv::Vec2d errors{}; + frc::Transform3d best{}; + std::optional alt{std::nullopt}; + + for (int tries = 0; tries < 2; tries++) { + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec, + reprojectionError); + + errors = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (tvecs.size() > 1) { + alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)), + RVecToRotation(rvecs[1])}; + } + + if (!std::isnan(errors[0])) { + break; + } else { + cv::Point2f pt = imagePoints[0]; + pt.x -= 0.001f; + pt.y -= 0.001f; + imagePoints[0] = pt; + } + } + + if (std::isnan(errors[0])) { + fmt::print("SolvePNP_Square failed!\n"); + } + if (alt) { + photon::PNPResult result; + result.best = best; + result.alt = alt.value(); + result.ambiguity = errors[0] / errors[1]; + result.bestReprojErr = errors[0]; + result.altReprojErr = errors[1]; + result.isPresent = true; + return result; + } else { + photon::PNPResult result; + result.best = best; + result.bestReprojErr = errors[0]; + result.isPresent = true; + return result; + } +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs{}; + std::vector tvecs{}; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + float error = 0; + frc::Transform3d best{}; + + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec, + reprojectionError); + + error = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (std::isnan(error)) { + fmt::print("SolvePNP_Square failed!\n"); + } + photon::PNPResult result; + result.best = best; + result.bestReprojErr = error; + result.isPresent = true; + return result; +} +} // namespace OpenCVHelp +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h new file mode 100644 index 0000000000..99e19bf2c3 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -0,0 +1,102 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include +#include +#include + +namespace photon { +class RotTrlTransform3d { + public: + RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl) + : trl(trl), rot(rot) {} + RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) + : trl(last.Translation() - initial.Translation().RotateBy(rot)), + rot(last.Rotation() - initial.Rotation()) {} + explicit RotTrlTransform3d(const frc::Transform3d& trf) + : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} + RotTrlTransform3d() + : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {} + + static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) { + return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse(); + } + + RotTrlTransform3d Inverse() const { + frc::Rotation3d invRot = -rot; + frc::Translation3d invTrl = -(trl.RotateBy(invRot)); + return RotTrlTransform3d{invRot, invTrl}; + } + + frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; } + + frc::Translation3d GetTranslation() const { return trl; } + + frc::Rotation3d GetRotation() const { return rot; } + + frc::Translation3d Apply(const frc::Translation3d& trlToApply) const { + return trlToApply.RotateBy(rot) + trl; + } + + std::vector ApplyTrls( + const std::vector& trls) const { + std::vector retVal; + retVal.reserve(trls.size()); + for (const auto& currentTrl : trls) { + retVal.emplace_back(Apply(currentTrl)); + } + return retVal; + } + + frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const { + return rotToApply + rot; + } + + std::vector ApplyTrls( + const std::vector& rots) const { + std::vector retVal; + retVal.reserve(rots.size()); + for (const auto& currentRot : rots) { + retVal.emplace_back(Apply(currentRot)); + } + return retVal; + } + + frc::Pose3d Apply(const frc::Pose3d& poseToApply) const { + return frc::Pose3d{Apply(poseToApply.Translation()), + Apply(poseToApply.Rotation())}; + } + + std::vector ApplyPoses( + const std::vector& poses) const { + std::vector retVal; + retVal.reserve(poses.size()); + for (const auto& currentPose : poses) { + retVal.emplace_back(Apply(currentPose)); + } + return retVal; + } + + private: + const frc::Translation3d trl; + const frc::Rotation3d rot; +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h new file mode 100644 index 0000000000..64392fdb11 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h @@ -0,0 +1,115 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include +#include + +#include "RotTrlTransform3d.h" + +namespace photon { +class TargetModel { + public: + TargetModel(units::meter_t width, units::meter_t height) + : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, height / 2.0}, + frc::Translation3d{0_m, -width / 2.0, height / 2.0}}), + isPlanar(true), + isSpherical(false) {} + + TargetModel(units::meter_t length, units::meter_t width, + units::meter_t height) + : TargetModel({ + frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0}, + }) {} + + explicit TargetModel(units::meter_t diameter) + : vertices({ + frc::Translation3d{0_m, -diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, -diameter / 2.0}, + frc::Translation3d{0_m, diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, diameter / 2.0}, + }), + isPlanar(false), + isSpherical(true) {} + + explicit TargetModel(const std::vector& verts) + : isSpherical(false) { + if (verts.size() <= 2) { + vertices = std::vector(); + isPlanar = false; + } else { + bool cornersPlanar = true; + for (const auto& corner : verts) { + if (corner.X() != 0_m) { + cornersPlanar = false; + } + } + isPlanar = cornersPlanar; + } + vertices = verts; + } + + std::vector GetFieldVertices( + const frc::Pose3d& targetPose) const { + RotTrlTransform3d basisChange{targetPose.Rotation(), + targetPose.Translation()}; + std::vector retVal; + retVal.reserve(vertices.size()); + for (const auto& vert : vertices) { + retVal.emplace_back(basisChange.Apply(vert)); + } + return retVal; + } + + static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl, + const frc::Translation3d& cameraTrl) { + frc::Translation3d relCam = cameraTrl - tgtTrl; + frc::Rotation3d orientToCam = frc::Rotation3d{ + 0_rad, + frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to(), + -relCam.Z().to()} + .Radians(), + frc::Rotation2d{relCam.X().to(), relCam.Y().to()} + .Radians()}; + return frc::Pose3d{tgtTrl, orientToCam}; + } + + std::vector GetVertices() const { return vertices; } + bool GetIsPlanar() const { return isPlanar; } + bool GetIsSpherical() const { return isSpherical; } + + private: + std::vector vertices; + bool isPlanar; + bool isSpherical; +}; + +static const TargetModel kAprilTag16h5{6_in, 6_in}; +static const TargetModel kAprilTag36h11{6.5_in, 6.5_in}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h new file mode 100644 index 0000000000..cb3ae3bdc5 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -0,0 +1,121 @@ +/* + * 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 . + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "OpenCVHelp.h" +#include "TargetModel.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { +namespace VisionEstimation { + +static std::vector GetVisibleLayoutTags( + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout) { + std::vector retVal{}; + for (const auto& tag : visTags) { + int id = tag.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + } + } + return retVal; +} + +static PNPResult EstimateCamPosePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + if (visTags.size() == 0) { + return PNPResult(); + } + + std::vector> corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return PNPResult{}; + } + + std::vector points = OpenCVHelp::CornersToPoints(corners); + + if (knownTags.size() == 1) { + PNPResult camToTag = OpenCVHelp::SolvePNP_Square( + cameraMatrix, distCoeffs, tagModel.GetVertices(), points); + if (!camToTag.isPresent) { + return PNPResult{}; + } + frc::Pose3d bestPose = + knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + frc::Pose3d altPose{}; + if (camToTag.ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + } + frc::Pose3d o{}; + PNPResult result{}; + result.best = frc::Transform3d{o, bestPose}; + result.alt = frc::Transform3d{o, altPose}; + result.ambiguity = camToTag.ambiguity; + result.bestReprojErr = camToTag.bestReprojErr; + result.altReprojErr = camToTag.altReprojErr; + return result; + } else { + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, + objectTrls, points); + if (!camToOrigin.isPresent) { + return PNPResult{}; + } else { + PNPResult result{}; + result.best = camToOrigin.best.Inverse(), + result.alt = camToOrigin.alt.Inverse(), + result.ambiguity = camToOrigin.ambiguity; + result.bestReprojErr = camToOrigin.bestReprojErr; + result.altReprojErr = camToOrigin.altReprojErr; + result.isPresent = true; + return result; + } + } +} + +} // namespace VisionEstimation +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h new file mode 100644 index 0000000000..ff647cd7cf --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h @@ -0,0 +1,100 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace photon { +class NTTopicSet { + public: + std::shared_ptr subTable; + nt::RawPublisher rawBytesEntry; + + nt::IntegerPublisher pipelineIndexPublisher; + nt::IntegerSubscriber pipelineIndexRequestSub; + + nt::BooleanTopic driverModeEntry; + nt::BooleanPublisher driverModePublisher; + nt::BooleanSubscriber driverModeSubscriber; + + nt::DoublePublisher latencyMillisEntry; + nt::BooleanPublisher hasTargetEntry; + nt::DoublePublisher targetPitchEntry; + nt::DoublePublisher targetYawEntry; + nt::DoublePublisher targetAreaEntry; + nt::DoubleArrayPublisher targetPoseEntry; + nt::DoublePublisher targetSkewEntry; + + nt::DoublePublisher bestTargetPosX; + nt::DoublePublisher bestTargetPosY; + + nt::IntegerTopic heartbeatTopic; + nt::IntegerPublisher heartbeatPublisher; + + nt::DoubleArrayPublisher cameraIntrinsicsPublisher; + nt::DoubleArrayPublisher cameraDistortionPublisher; + + void UpdateEntries() { + nt::PubSubOptions options; + options.periodic = 0.01; + options.sendAll = true; + rawBytesEntry = + subTable->GetRawTopic("rawBytes").Publish("rawBytes", options); + + pipelineIndexPublisher = + subTable->GetIntegerTopic("pipelineIndexState").Publish(); + pipelineIndexRequestSub = + subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0); + + driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish(); + driverModeSubscriber = + subTable->GetBooleanTopic("driverModeRequest").Subscribe(0); + + driverModeSubscriber.GetTopic().Publish().SetDefault(false); + + latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish(); + hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish(); + + targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish(); + targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish(); + targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish(); + targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish(); + targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish(); + + bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish(); + bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish(); + + heartbeatTopic = subTable->GetIntegerTopic("heartbeat"); + heartbeatPublisher = heartbeatTopic.Publish(); + + cameraIntrinsicsPublisher = + subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish(); + cameraDistortionPublisher = + subTable->GetDoubleArrayTopic("cameraDistortion").Publish(); + } + + private: +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index e97f6c8d71..49f73ed53b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -27,15 +27,15 @@ class PNPResult { public: // This could be wrapped in an std::optional, but chose to do it this way to // mirror Java - bool isPresent; + bool isPresent{false}; - frc::Transform3d best; - double bestReprojErr; + frc::Transform3d best{}; + double bestReprojErr{0}; - frc::Transform3d alt; - double altReprojErr; + frc::Transform3d alt{}; + double altReprojErr{0}; - double ambiguity; + double ambiguity{0}; bool operator==(const PNPResult& other) const; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 7eb18d125f..feba565910 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -112,8 +112,10 @@ class PhotonTrackedTarget { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above - * 0.2 are likely to be ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. + * This is betweeen 0 and 1 (0 being no ambiguity, and 1 meaning both have the + * same reprojection error). Numbers above 0.2 are likely to be ambiguous. -1 + * if invalid. */ double GetPoseAmbiguity() const { return poseAmbiguity; } diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 881e04a114..51263e12bc 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } allprojects { diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore new file mode 100644 index 0000000000..34878ab18c --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..a6e62683de --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md new file mode 100644 index 0000000000..3d5a824cad --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle new file mode 100644 index 0000000000..8eeef20126 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -0,0 +1,121 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" + + id "com.dorongold.task-tree" version "2.1.0" +} + +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' +wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' + +repositories { + mavenLocal() + jcenter() +} + +apply from: "${rootDir}/../shared/examples_common.gradle" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamOrDefault(5940) + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcCpp + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Set to true to run simulation in debug mode +wpi.cpp.debugSimulation = false + +// Default enable simgui +wpi.sim.addGui().defaultEnabled = true +// Enable DS but not by default +wpi.sim.addDriverstation() + +model { + components { + frcUserProgram(NativeExecutableSpec) { + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + + // Set deploy task to deploy this component + deployArtifact.component = it + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + // Enable simulation for this component + wpi.sim.enable(it) + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew @@ -0,0 +1,241 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle new file mode 100644 index 0000000000..44fbca7512 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json new file mode 100644 index 0000000000..c4b7efd3d8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json new file mode 100644 index 0000000000..1605887709 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json @@ -0,0 +1,64 @@ +{ + "Docking": { + "Data": [] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "910", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "1550", + "xpos": "-1602", + "ypos": "79" + } + }, + "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "199,200", + "Size": "1342,628" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "5,540", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "359,95", + "Size": "796,240" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "865,52", + "Size": "830,620" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..e1ba9acce8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json @@ -0,0 +1,57 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "width": 0.30000001192092896 + }, + "apriltag": { + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "bottom": 544, + "builtin": "2023 Charged Up", + "cameras": { + "arrowSize": 19, + "arrowWeight": 1.0, + "style": "Hidden" + }, + "height": 8.013679504394531, + "left": 46, + "right": 1088, + "top": 36, + "visibleTargetPoses": { + "image": "tag-blue.png" + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drive": { + "open": true + }, + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp new file mode 100644 index 0000000000..4505740955 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp @@ -0,0 +1,123 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "Robot.h" + +#include + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + drivetrain.Periodic(); + + auto visionEst = vision.GetEstimatedGlobalPose(); + if (visionEst.has_value()) { + auto est = visionEst.value(); + auto estPose = est.estimatedPose.ToPose2d(); + auto estStdDevs = vision.GetEstimationStdDevs(estPose); + drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp, + estStdDevs); + } + + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() { + autoTimer.Restart(); + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + +void Robot::AutonomousPeriodic() { + if (autoTimer.Get() < 10_s) { + drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); + } else { + autoTimer.Stop(); + drivetrain.Stop(); + } +} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() {} + +void Robot::TeleopPeriodic() { + double forward = -controller.GetLeftY() * kDriveSpeed; + if (std::abs(forward) < 0.1) { + forward = 0; + } + forward = forwardLimiter.Calculate(forward); + + double strafe = -controller.GetLeftX() * kDriveSpeed; + if (std::abs(strafe) < 0.1) { + strafe = 0; + } + strafe = strafeLimiter.Calculate(strafe); + + double turn = -controller.GetRightX() * kDriveSpeed; + if (std::abs(turn) < 0.1) { + turn = 0; + } + turn = turnLimiter.Calculate(turn); + + drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, + strafe * constants::Swerve::kMaxLinearSpeed, + turn * constants::Swerve::kMaxAngularSpeed, true); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..bf33d7189d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,211 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDrive.h" + +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, openLoop, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); } + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp) { + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); +} + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); +} + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..781c28a338 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,146 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveModule.h" + +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h new file mode 100644 index 0000000000..70be37d406 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +static constexpr units::meter_t kTrackWidth{18.5_in}; +static constexpr units::meter_t kTrackLength{18.5_in}; +static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +static constexpr units::meter_t kWheelDiameter{4_in}; +static constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +static constexpr double kDriveGearRatio = 6.75; +static constexpr double kSteerGearRatio = 12.8; + +static constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +static constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +static constexpr double kDriveKP = 1.0; +static constexpr double kDriveKI = 0.0; +static constexpr double kDriveKD = 0.0; + +static constexpr double kSteerKP = 20.0; +static constexpr double kSteerKI = 0.0; +static constexpr double kSteerKD = 0.25; + +static const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +static const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +static const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +static const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photon-lib/src/main/driver/include/driverheader.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h similarity index 51% rename from photon-lib/src/main/driver/include/driverheader.h rename to photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h index eb2a4de816..7a3697494a 100644 --- a/photon-lib/src/main/driver/include/driverheader.h +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h @@ -24,6 +24,39 @@ #pragma once -extern "C" { -void c_doThing(void); -} // extern "C" +#include +#include +#include +#include + +#include "Vision.h" +#include "subsystems/SwerveDrive.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; + + private: + SwerveDrive drivetrain{}; + Vision vision{}; + frc::XboxController controller{0}; + frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; + frc::Timer autoTimer{}; + double kDriveSpeed{0.6}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h new file mode 100644 index 0000000000..619b34ade7 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h @@ -0,0 +1,152 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class Vision { + public: + Vision() { + photonEstimator.SetMultiTagFallbackStrategy( + photon::PoseStrategy::LOWEST_AMBIGUITY); + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(15_Hz); + cameraProp->SetAvgLatency(50_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = std::make_shared(camera.get(), + *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), robotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { + return camera->GetLatestResult(); + } + + std::optional GetEstimatedGlobalPose() { + auto visionEst = photonEstimator.Update(); + units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp(); + bool newResult = + units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s; + if (frc::RobotBase::IsSimulation()) { + if (visionEst.has_value()) { + GetSimDebugField() + .GetObject("VisionEstimation") + ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + } else { + if (newResult) { + GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); + } + } + } + if (newResult) { + lastEstTimestamp = latestTimestamp; + } + return visionEst; + } + + Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix estStdDevs = + constants::Vision::kSingleTagStdDevs; + auto targets = GetLatestResult().GetTargets(); + int numTags = 0; + units::meter_t avgDist = 0_m; + for (const auto& tgt : targets) { + auto tagPose = + photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); + if (tagPose.has_value()) { + numTags++; + avgDist += tagPose.value().ToPose2d().Translation().Distance( + estimatedPose.Translation()); + } + } + if (numTags == 0) { + return estStdDevs; + } + avgDist /= numTags; + if (numTags > 1) { + estStdDevs = constants::Vision::kMultiTagStdDevs; + } + if (numTags == 1 && avgDist > 4_m) { + estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()) + .finished(); + } else { + estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30)); + } + return estStdDevs; + } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m}, + frc::Rotation3d{}}; + photon::PhotonPoseEstimator photonEstimator{ + LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + photon::PhotonCamera{"photonvision"}, robotToCam}; + std::shared_ptr camera{photonEstimator.GetCamera()}; + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + units::second_t lastEstTimestamp{0_s}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..1e3d26b32f --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photon-lib/src/main/driver/cpp/driversource.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp similarity index 85% rename from photon-lib/src/main/driver/cpp/driversource.cpp rename to photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp index 039fde6f8e..031d1ce96b 100644 --- a/photon-lib/src/main/driver/cpp/driversource.cpp +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp @@ -22,8 +22,13 @@ * SOFTWARE. */ -#include "driverheader.h" +#include -extern "C" { -void c_doThing(void) {} -} // extern "C" +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png differ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000..04b9e4f7b7 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png differ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000..63029fcf20 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png differ diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 1c92e043f9..fb2207b6c5 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } apply from: "examples.gradle" diff --git a/shared/common.gradle b/shared/common.gradle index dc3331ff16..7cbf858b22 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -7,28 +7,22 @@ java { targetCompatibility = JavaVersion.VERSION_11 } -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() +wpilibTools.deps.wpilibVersion = wpilibVersion -def nativeConfigName = 'wpilibTestNative' -def nativeConfig = configurations.create(nativeConfigName) +// Tell gradlerio what version of things to use (that we care about) +// See: https://github.com/wpilibsuite/GradleRIO/blob/main/src/main/java/edu/wpi/first/gradlerio/wpi/WPIVersionsExtension.java +wpi.getVersions().getOpencvVersion().convention(openCVversion); +wpi.getVersions().getWpilibVersion().convention(wpilibVersion); +wpi.getVersions().getWpimathVersion().convention(wpimathVersion); -def nativeTasks = wpilibTools.createExtractionTasks { - configurationName = nativeConfigName -} +dependencies { + implementation project(':photon-targeting') -nativeTasks.addToSourceSetResources(sourceSets.main) + implementation "io.javalin:javalin:$javalinVersion" -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") -nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") -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" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) + implementation 'org.msgpack:msgpack-core:0.9.0' + implementation 'org.msgpack:jackson-dataformat-msgpack:0.9.0' -dependencies { - // WPILib deps implementation wpilibTools.deps.wpilibJava("wpiutil") implementation wpilibTools.deps.wpilibJava("cameraserver") implementation wpilibTools.deps.wpilibJava("cscore") @@ -38,27 +32,25 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - // Jackson implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() - // Apache commons - implementation group: "org.apache.commons", name: "commons-lang3", version: "3.12.0" - implementation group: "commons-io", name: "commons-io", version: "2.11.0" - implementation group: "commons-cli", name: "commons-cli", version: "1.5.0" - implementation "org.apache.commons:commons-collections4:4.4" - implementation "org.apache.commons:commons-exec:1.3" - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); - testImplementation("org.junit.jupiter:junit-jupiter:5.8.2") + implementation "commons-io:commons-io:2.11.0" + implementation "commons-cli:commons-cli:1.5.0" + implementation "org.apache.commons:commons-lang3:3.12.0" + implementation "org.apache.commons:commons-collections4:4.4" + implementation "org.apache.commons:commons-exec:1.3" - // wpilib serde - implementation 'us.hebi.quickbuf:quickbuf-runtime:1.3.2' + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0' } test { diff --git a/shared/config.gradle b/shared/config.gradle index 43300e26cc..4044d7e3ff 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -9,11 +9,12 @@ nativeUtils.withCrossLinuxArm64() // Configure WPI dependencies. nativeUtils.wpi.configureDependencies { wpiVersion = wpilibVersion + wpimathVersion = wpimathVersion opencvYear = 'frc2024' opencvVersion = openCVversion - googleTestYear = "frc2023" - niLibVersion = "2024.1.1" - googleTestVersion = "1.12.1-2" + googleTestYear = "frc2024" + googleTestVersion = "1.14.0-1" + niLibVersion = "2024.2.1" } // Configure warnings and errors diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 6792f61c73..55f72d0f60 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -97,7 +97,32 @@ test { finalizedBy jacocoTestReport } +wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() + dependencies { + if(project.hasProperty('includePhotonTargeting')) { + implementation project(":photon-targeting") + } + + implementation wpilibTools.deps.wpilibJava("wpiutil") + implementation wpilibTools.deps.wpilibJava("cameraserver") + implementation wpilibTools.deps.wpilibJava("cscore") + implementation wpilibTools.deps.wpilibJava("wpinet") + implementation wpilibTools.deps.wpilibJava("wpimath") + implementation wpilibTools.deps.wpilibJava("ntcore") + implementation wpilibTools.deps.wpilibJava("hal") + implementation wpilibTools.deps.wpilibJava("wpilibj") + implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") + implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) + + implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() + + implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0' testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0' diff --git a/shared/publish.gradle b/shared/javacpp/publish.gradle similarity index 100% rename from shared/publish.gradle rename to shared/javacpp/publish.gradle diff --git a/shared/setupBuild.gradle b/shared/javacpp/setupBuild.gradle similarity index 66% rename from shared/setupBuild.gradle rename to shared/javacpp/setupBuild.gradle index b90f87e154..dd8a5bec32 100644 --- a/shared/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -5,6 +5,26 @@ apply plugin: 'edu.wpi.first.NativeUtils' apply from: "${rootDir}/shared/config.gradle" apply from: "${rootDir}/shared/javacommon.gradle" +wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() + +def nativeConfigName = 'wpilibNatives' +def nativeConfig = configurations.create(nativeConfigName) + +def nativeTasks = wpilibTools.createExtractionTasks { + configurationName = nativeConfigName +} + +nativeTasks.addToSourceSetResources(sourceSets.main) + +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") +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" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) + // Windows specific functionality to export all symbols from a binary automatically nativeUtils { exportsConfigs { @@ -23,6 +43,9 @@ model { } exportedHeaders { srcDirs 'src/main/native/include' + if (project.hasProperty('generatedHeaders')) { + srcDir generatedHeaders + } include "**/*.h" } } @@ -65,6 +88,8 @@ model { } } + nativeUtils.useRequiredLibrary(it, "cscore_shared") + nativeUtils.useRequiredLibrary(it, "cameraserver_shared") nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") @@ -73,4 +98,4 @@ model { } } -apply from: "${rootDir}/shared/publish.gradle" +apply from: "${rootDir}/shared/javacpp/publish.gradle"