diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 71c928d4b..5d8947820 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -14,7 +14,7 @@ on: jobs: # This job builds the client (web view). - build-client: + photonclient-build: # Let all steps run within the photon-client dir. defaults: @@ -49,7 +49,7 @@ jobs: name: built-client path: photon-client/dist/ - build-server: + photon-build-all: # The type of runner that the job will run on. runs-on: ubuntu-latest @@ -89,7 +89,7 @@ jobs: with: file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml - build-offline-docs: + photonserver-build-offline-docs: runs-on: ubuntu-latest steps: @@ -125,8 +125,8 @@ jobs: name: built-docs path: build/html - build-package: - needs: [build-client, build-server, build-offline-docs] + photon-build-package: + needs: [photonclient-build, photon-build-all, photonserver-build-offline-docs] # The type of runner that the job will run on. runs-on: ubuntu-latest @@ -177,7 +177,7 @@ jobs: photon-server/build/libs/*.jar if: github.event_name == 'push' - check-lint: + photonserver-check-lint: # The type of runner that the job will run on. runs-on: ubuntu-latest @@ -195,9 +195,9 @@ jobs: chmod +x gradlew ./gradlew spotlessCheck - release: + photon-release: if: startsWith(github.ref, 'refs/tags/v') - needs: [build-package] + needs: [photon-build-package] runs-on: ubuntu-latest steps: - uses: actions/download-artifact@v2 @@ -208,3 +208,87 @@ jobs: files: '**/*' env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + # Building photonlib + photonlib-build-host: + strategy: + fail-fast: false + matrix: + include: + - os: windows-latest + artifact-name: Win64 + - os: macos-latest + artifact-name: macOS + - os: ubuntu-latest + artifact-name: Linux + + runs-on: ${{ matrix.os }} + name: "Photonlib - Build - ${{ matrix.artifact-name }}" + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-java@v1 + with: + java-version: 11 + - run: | + chmod +x gradlew + ./gradlew photon-lib:build + - run: ./gradlew photonlib:publish + name: Publish + env: + ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} + if: github.event_name == 'push' + + photonlib-build-docker: + strategy: + fail-fast: false + matrix: + include: + - container: wpilib/roborio-cross-ubuntu:2020-18.04 + artifact-name: Athena + - container: wpilib/raspbian-cross-ubuntu:10-18.04 + artifact-name: Raspbian + - container: wpilib/aarch64-cross-ubuntu:bionic-18.04 + artifact-name: Aarch64 + + runs-on: ubuntu-latest + container: ${{ matrix.container }} + name: "Photonlib - Build - ${{ matrix.artifact-name }}" + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-java@v1 + with: + java-version: 11 + - run: | + chmod +x gradlew + ./gradlew photon-lib:build + - run: | + chmod +x gradlew + ./gradlew photon-lib:publish + env: + ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} + if: github.event_name == 'push' + + photonlib-wpiformat: + name: "wpiformat" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: Fetch all history and metadata + run: | + git fetch --prune --unshallow + git checkout -b pr + git branch -f master origin/master + - name: Set up Python 3.8 + uses: actions/setup-python@v2 + with: + python-version: 3.8 + - name: Install clang-format + run: sudo apt-get update -q && sudo apt-get install clang-format-10 + - name: Install wpiformat + run: pip3 install wpiformat + - name: Run + run: | + ls -la + wpiformat -clang 10 -f photon-lib + - name: Check Output + run: git --no-pager diff --exit-code HEAD diff --git a/.gitignore b/.gitignore index 3e8fcce9f..21cd24014 100644 --- a/.gitignore +++ b/.gitignore @@ -131,3 +131,6 @@ photon-server/src/main/generated/native/include/org_photonvision_raspi_PicamJNI. .gradle/* photonvision_config build/spotlessJava +build/* +build +photon-lib/src/main/java/org/photonvision/PhotonVersion.java diff --git a/photon-core/build.gradle b/photon-core/build.gradle index fd6eb19a0..44674159d 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -1,7 +1,12 @@ +import java.time.LocalDateTime +import java.time.format.DateTimeFormatter + apply from: '../common.gradle' -apply from: 'versioningHelper.gradle' +apply from: '../versioningHelper.gradle' dependencies { + implementation project(':photon-targeting') + implementation 'io.javalin:javalin:3.7.0' implementation 'org.msgpack:msgpack-core:0.8.20' @@ -17,3 +22,25 @@ dependencies { // Zip compile 'org.zeroturnaround:zt-zip:1.14' } + +task writeCurrentVersionJava { + String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now()) + File versionFile = new File(java.nio.file.Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java") + .toAbsolutePath().toString()) + versionFile.delete() + versionFile << "package org.photonvision;\n" + + "\n" + + "/*\n" + + " * Autogenerated file! Do not manually edit this file. This version is regenerated\n" + + " * any time the publish task is run, or when this file is deleted.\n" + + " */\n" + + "\n" + + "@SuppressWarnings(\"ALL\")\n" + + "public final class PhotonVersion {\n" + + " public static final String versionString = \"${versionString}\";\n" + + " public static final String buildDate = \"${date}\";\n" + + " public static final boolean isRelease = !versionString.startsWith(\"dev\");\n" + + "}" +} + +build.dependsOn writeCurrentVersionJava diff --git a/photon-core/gradle/wrapper/gradle-wrapper.jar b/photon-core/gradle/wrapper/gradle-wrapper.jar deleted file mode 100644 index cc4fdc293..000000000 Binary files a/photon-core/gradle/wrapper/gradle-wrapper.jar and /dev/null differ diff --git a/photon-core/gradle/wrapper/gradle-wrapper.properties b/photon-core/gradle/wrapper/gradle-wrapper.properties deleted file mode 100644 index b16a683a7..000000000 --- a/photon-core/gradle/wrapper/gradle-wrapper.properties +++ /dev/null @@ -1,6 +0,0 @@ -distributionBase=GRADLE_USER_HOME -distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.7.1-bin.zip -distributionSha256Sum=3239b5ed86c3838a37d983ac100573f64c1f3fd8e1eb6c89fa5f9529b5ec091d -zipStoreBase=GRADLE_USER_HOME -zipStorePath=wrapper/dists diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 0fb875283..710aebb25 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -20,13 +20,17 @@ package org.photonvision.common.dataflow.networktables; import edu.wpi.first.networktables.EntryNotification; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; +import java.util.ArrayList; +import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.vision.pipeline.result.CVPipelineResult; -import org.photonvision.vision.pipeline.result.SimplePipelineResult; +import org.photonvision.vision.target.TrackedTarget; public class NTDataPublisher implements CVPipelineResultConsumer { @@ -163,7 +167,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer { @Override public void accept(CVPipelineResult result) { - var simplified = new SimplePipelineResult(result); + var simplified = + new PhotonPipelineResult( + result.getLatencyMillis(), simpleFromTrackedTargets(result.targets)); Packet packet = new Packet(simplified.getPacketSize()); simplified.populatePacket(packet); @@ -201,4 +207,14 @@ public class NTDataPublisher implements CVPipelineResultConsumer { } rootTable.getInstance().flush(); } + + public static List simpleFromTrackedTargets(List targets) { + var ret = new ArrayList(); + for (var t : targets) { + ret.add( + new PhotonTrackedTarget( + t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget())); + } + return ret; + } } diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index da42f72c8..fbaebc4cc 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -27,7 +27,6 @@ import org.photonvision.common.dataflow.networktables.NTDataChangeListener; import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.hardware.GPIO.CustomGPIO; import org.photonvision.common.hardware.GPIO.pi.PigpioSocket; -import org.photonvision.common.hardware.VisionLED.VisionLEDMode; import org.photonvision.common.hardware.metrics.MetricsBase; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; @@ -91,7 +90,7 @@ public class HardwareManager { pigpioSocket); ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode"); - ledModeEntry.setNumber(VisionLEDMode.VLM_DEFAULT.value); + ledModeEntry.setNumber(VisionLEDMode.kDefault.value); ledModeListener = visionLED == null ? null diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java index ba9ff9599..880619e23 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java @@ -40,7 +40,7 @@ public class VisionLED { private final int brightnessMax; private final PigpioSocket pigpioSocket; - private VisionLEDMode currentLedMode = VisionLEDMode.VLM_DEFAULT; + private VisionLEDMode currentLedMode = VisionLEDMode.kDefault; private BooleanSupplier pipelineModeSupplier; private int mappedBrightnessPercentage; @@ -111,7 +111,7 @@ public class VisionLED { } public void setState(boolean on) { - setInternal(on ? VisionLEDMode.VLM_ON : VisionLEDMode.VLM_OFF, false); + setInternal(on ? VisionLEDMode.kOn : VisionLEDMode.kOff, false); } void onLedModeChange(EntryNotification entryNotification) { @@ -120,20 +120,20 @@ public class VisionLED { VisionLEDMode newLedMode; switch (newLedModeRaw) { case -1: - newLedMode = VisionLEDMode.VLM_DEFAULT; + newLedMode = VisionLEDMode.kDefault; break; case 0: - newLedMode = VisionLEDMode.VLM_OFF; + newLedMode = VisionLEDMode.kOff; break; case 1: - newLedMode = VisionLEDMode.VLM_ON; + newLedMode = VisionLEDMode.kOn; break; case 2: - newLedMode = VisionLEDMode.VLM_BLINK; + newLedMode = VisionLEDMode.kBlink; break; default: logger.warn("User supplied invalid LED mode, falling back to Default"); - newLedMode = VisionLEDMode.VLM_DEFAULT; + newLedMode = VisionLEDMode.kDefault; break; } setInternal(newLedMode, true); @@ -145,16 +145,16 @@ public class VisionLED { if (fromNT) { switch (newLedMode) { - case VLM_DEFAULT: + case kDefault: setStateImpl(pipelineModeSupplier.getAsBoolean()); break; - case VLM_OFF: + case kOff: setStateImpl(false); break; - case VLM_ON: + case kOn: setStateImpl(true); break; - case VLM_BLINK: + case kBlink: blinkImpl(85, -1); break; } @@ -166,15 +166,15 @@ public class VisionLED { + newLedMode.toString() + "\""); } else { - if (currentLedMode == VisionLEDMode.VLM_DEFAULT) { + if (currentLedMode == VisionLEDMode.kDefault) { switch (newLedMode) { - case VLM_DEFAULT: + case kDefault: setStateImpl(pipelineModeSupplier.getAsBoolean()); break; - case VLM_OFF: + case kOff: setStateImpl(false); break; - case VLM_ON: + case kOn: setStateImpl(true); break; } @@ -182,32 +182,4 @@ public class VisionLED { logger.info("Changing LED internal state to " + newLedMode.toString()); } } - - public enum VisionLEDMode { - VLM_DEFAULT(-1), - VLM_OFF(0), - VLM_ON(1), - VLM_BLINK(2); - - public final int value; - - VisionLEDMode(int value) { - this.value = value; - } - - @Override - public String toString() { - switch (this) { - case VLM_DEFAULT: - return "Default"; - case VLM_OFF: - return "Off"; - case VLM_ON: - return "On"; - case VLM_BLINK: - return "Blink"; - } - return ""; - } - } } diff --git a/photon-core/versioningHelper.gradle b/photon-core/versioningHelper.gradle deleted file mode 100644 index 84fa29875..000000000 --- a/photon-core/versioningHelper.gradle +++ /dev/null @@ -1,46 +0,0 @@ -import java.time.LocalDateTime -import java.time.format.DateTimeFormatter -import java.nio.file.Path - -gradle.allprojects { - ext.getCurrentVersion = { -> - def stdout = new ByteArrayOutputStream() - String tagIsh - try { - exec { - commandLine 'git', 'describe', '--tags', '--exclude="Dev"' - standardOutput = stdout - } - tagIsh = stdout.toString().trim().toLowerCase() - } catch(Exception e) { - tagIsh = "dev-Unknown" - } - boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*") - if(isDev) tagIsh = "dev-" + tagIsh - println("Picked up version: " + tagIsh) - return tagIsh - } - ext.versionString = getCurrentVersion() -} - -task writeCurrentVersionJava { - String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now()) - File versionFile = new File(Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java") - .toAbsolutePath().toString()) - versionFile.delete() - versionFile << "package org.photonvision;\n" + - "\n" + - "/*\n" + - " * Autogenerated file! Do not manually edit this file. This version is regenerated\n" + - " * any time the publish task is run, or when this file is deleted.\n" + - " */\n" + - "\n" + - "@SuppressWarnings(\"ALL\")\n" + - "public final class PhotonVersion {\n" + - " public static final String versionString = \"${versionString}\";\n" + - " public static final String buildDate = \"${date}\";\n" + - " public static final boolean isRelease = !versionString.startsWith(\"dev\");\n" + - "}" -} - -build.dependsOn writeCurrentVersionJava diff --git a/photon-lib/.clang-format b/photon-lib/.clang-format new file mode 100644 index 000000000..14ff7a4a8 --- /dev/null +++ b/photon-lib/.clang-format @@ -0,0 +1,167 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: WithoutElse +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + - Regex: '.*' + Priority: 3 + SortPriority: 0 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Auto +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +... diff --git a/photon-lib/.styleguide b/photon-lib/.styleguide new file mode 100644 index 000000000..645ca1a39 --- /dev/null +++ b/photon-lib/.styleguide @@ -0,0 +1,20 @@ +cppHeaderFileInclude { + \.h$ + \.hpp$ + \.inc$ + \.inl$ +} + +cppSrcFileInclude { + \.cpp$ +} + +includeProject { + ^photonLib/ +} + +includeOtherLibs { + ^frc/ + ^units/ + ^wpi/ +} diff --git a/photon-lib/.styleguide-license b/photon-lib/.styleguide-license new file mode 100644 index 000000000..bcc3fc01e --- /dev/null +++ b/photon-lib/.styleguide-license @@ -0,0 +1,16 @@ +/* + * 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 . + */ diff --git a/photon-lib/LICENSE b/photon-lib/LICENSE new file mode 100644 index 000000000..f288702d2 --- /dev/null +++ b/photon-lib/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + 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 . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/photon-lib/README.md b/photon-lib/README.md new file mode 100644 index 000000000..34e975fdb --- /dev/null +++ b/photon-lib/README.md @@ -0,0 +1,3 @@ +# PhotonLib + +The vendor dependency for [PhotonVision](https://github.com/photonvision/photonvision). Just add the vendor JSON to your robot project and you're good! diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle new file mode 100644 index 000000000..a8a1f0fab --- /dev/null +++ b/photon-lib/build.gradle @@ -0,0 +1,129 @@ +plugins { + id 'cpp' + id 'java' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2' + id 'google-test-test-suite' + id 'edu.wpi.first.NativeUtils' version '2020.10.1' + id 'edu.wpi.first.GradleJni' version '0.10.1' + id 'edu.wpi.first.GradleVsCode' version '0.12.0' +} + +repositories { + mavenCentral() + maven { url "https://frcmaven.wpi.edu/artifactory/development" } +} + +if (project.hasProperty('releaseMode')) { + wpilibRepositories.addAllReleaseRepositories(project) +} else { + wpilibRepositories.addAllDevelopmentRepositories(project) +} + +apply from: '../versioningHelper.gradle' + +ext { + pubVersion = versionString + wpilibVersion = '2020.3.2-99-g9f4de91' +} + +// Apply C++ configuration +apply from: 'config.gradle' + +test { + useJUnitPlatform() +} + +// Apply Java configuration +dependencies { + + // TODO C++ + compile project(':photon-core') + compile project(':photon-targeting') + + implementation 'edu.wpi.first.cscore:cscore-java:2020.+' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2020.+' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2020.+' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2020.+' + implementation 'edu.wpi.first.wpimath:wpimath-java:2020.+' + implementation 'edu.wpi.first.hal:hal-java:2020.+' + implementation 'edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:3.4.7-2' + + implementation "edu.wpi.first.ntcore:ntcore-java:$wpilibVersion" + compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxaarch64bionic" + compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxraspbian" + compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxx86-64" + compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:osxx86-64" + compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:windowsx86-64" + + testImplementation("org.junit.jupiter:junit-jupiter-api:5.6.2") + testImplementation("org.junit.jupiter:junit-jupiter-params:5.6.2") + testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.6.2") +} + +// Set up exports properly +nativeUtils { + exportsConfigs { + // Main library is just default empty. This will export everything + Photon { + } + } +} + +model { + components { + Photon(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs 'src/main/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include' + } + } + } + nativeUtils.useRequiredLibrary(it, 'wpilib_shared') + } + } + testSuites { + cppTest(GoogleTestTestSuiteSpec) { + testing $.components.Photon + + sources.cpp { + source { + srcDir 'src/test/native/cpp' + include '**/*.cpp' + } + } + + nativeUtils.useRequiredLibrary(it, 'wpilib_executable_shared') + nativeUtils.useRequiredLibrary(it, 'googletest_static') + } + } +} + +def photonlibFileInput = file("src/generate/photonlib.json.in") +ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json") + +task generateVendorJson() { + description = 'Generates the vendor JSON file' + group = 'PhotonVision' + + outputs.file photonlibFileOutput + inputs.file photonlibFileInput + + doLast { + println "Writing version ${pubVersion} to $photonlibFileOutput" + + if (photonlibFileOutput.exists()) { + photonlibFileOutput.delete() + } + def read = photonlibFileInput.text.replace('${photon_version}', pubVersion) + photonlibFileOutput.write(read) + } +} + +build.dependsOn generateVendorJson + +apply from: 'publish.gradle' diff --git a/photon-lib/clang-format.sh b/photon-lib/clang-format.sh new file mode 100644 index 000000000..0b72f3b51 --- /dev/null +++ b/photon-lib/clang-format.sh @@ -0,0 +1,62 @@ +#!/bin/bash +################################################################################ +# Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions. +# See https://llvm.org/LICENSE.txt for license information. +# SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception +################################################################################ +# +# This script will install the llvm toolchain on the different +# Debian and Ubuntu versions + +set -eux + +# read optional command line argument +LLVM_VERSION=10 +if [ "$#" -eq 1 ]; then + LLVM_VERSION=$1 +fi + +DISTRO=$(lsb_release -is) +VERSION=$(lsb_release -sr) +DIST_VERSION="${DISTRO}_${VERSION}" + +if [[ $EUID -ne 0 ]]; then + echo "This script must be run as root!" + exit 1 +fi + +declare -A LLVM_VERSION_PATTERNS +LLVM_VERSION_PATTERNS[9]="-9" +LLVM_VERSION_PATTERNS[10]="-10" +LLVM_VERSION_PATTERNS[11]="" + +if [ ! ${LLVM_VERSION_PATTERNS[$LLVM_VERSION]+_} ]; then + echo "This script does not support LLVM version $LLVM_VERSION" + exit 3 +fi + +LLVM_VERSION_STRING=${LLVM_VERSION_PATTERNS[$LLVM_VERSION]} + +# find the right repository name for the distro and version +case "$DIST_VERSION" in + Debian_9* ) REPO_NAME="deb http://apt.llvm.org/stretch/ llvm-toolchain-stretch$LLVM_VERSION_STRING main" ;; + Debian_10* ) REPO_NAME="deb http://apt.llvm.org/buster/ llvm-toolchain-buster$LLVM_VERSION_STRING main" ;; + Debian_unstable ) REPO_NAME="deb http://apt.llvm.org/unstable/ llvm-toolchain$LLVM_VERSION_STRING main" ;; + Debian_testing ) REPO_NAME="deb http://apt.llvm.org/unstable/ llvm-toolchain$LLVM_VERSION_STRING main" ;; + Ubuntu_16.04 ) REPO_NAME="deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial$LLVM_VERSION_STRING main" ;; + Ubuntu_18.04 ) REPO_NAME="deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic$LLVM_VERSION_STRING main" ;; + Ubuntu_18.10 ) REPO_NAME="deb http://apt.llvm.org/cosmic/ llvm-toolchain-cosmic$LLVM_VERSION_STRING main" ;; + Ubuntu_19.04 ) REPO_NAME="deb http://apt.llvm.org/disco/ llvm-toolchain-disco$LLVM_VERSION_STRING main" ;; + Ubuntu_19.10 ) REPO_NAME="deb http://apt.llvm.org/eoan/ llvm-toolchain-eoan$LLVM_VERSION_STRING main" ;; + Ubuntu_20.04 ) REPO_NAME="deb http://apt.llvm.org/focal/ llvm-toolchain-focal$LLVM_VERSION_STRING main" ;; + * ) + echo "Distribution '$DISTRO' in version '$VERSION' is not supported by this script (${DIST_VERSION})." + exit 2 +esac + + +# install everything +wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | apt-key add - +add-apt-repository "${REPO_NAME}" +apt-get update +apt-get install -y clang-format-$LLVM_VERSION diff --git a/photon-lib/config.gradle b/photon-lib/config.gradle new file mode 100644 index 000000000..ec82167db --- /dev/null +++ b/photon-lib/config.gradle @@ -0,0 +1,181 @@ +import org.gradle.internal.os.OperatingSystem + +nativeUtils.addWpiNativeUtils() +nativeUtils.withRoboRIO() +nativeUtils.withRaspbian() +nativeUtils.withBionic() + +nativeUtils { + wpi { + configureDependencies { + wpiVersion = "2020.+" + niLibVersion = "2020.10.1" + wpimathVersion = "2020.+" + opencvVersion = "3.4.7-2" + niLibVersion = "2020.10.1" + googleTestVersion = "1.9.0-3-437e100" + opencvVersion = "3.4.7-2" + googleTestVersion = "1.9.0-3-437e100" + } + } +} + +nativeUtils.wpi.addWarnings() +nativeUtils.wpi.addWarningsAsErrors() + +nativeUtils.setSinglePrintPerPlatform() + +model { + // components { + // all { + // targetPlatform nativeUtils.wpi.platforms.roborio + // } + // } + // Uncomment this, and remove above lines to enable builds for all platforms + components { + all { + nativeUtils.useAllPlatforms(it) + } + } + binaries { + withType(NativeBinarySpec).all { + nativeUtils.usePlatformArguments(it) + } + } +} + +ext.appendDebugPathToBinaries = { binaries-> + binaries.withType(StaticLibraryBinarySpec) { + if (it.buildType.name.contains('debug')) { + def staticFileDir = it.staticLibraryFile.parentFile + def staticFileName = it.staticLibraryFile.name + def staticFileExtension = staticFileName.substring(staticFileName.lastIndexOf('.')) + staticFileName = staticFileName.substring(0, staticFileName.lastIndexOf('.')) + staticFileName = staticFileName + 'd' + staticFileExtension + def newStaticFile = new File(staticFileDir, staticFileName) + it.staticLibraryFile = newStaticFile + } + } + binaries.withType(SharedLibraryBinarySpec) { + if (it.buildType.name.contains('debug')) { + def sharedFileDir = it.sharedLibraryFile.parentFile + def sharedFileName = it.sharedLibraryFile.name + def sharedFileExtension = sharedFileName.substring(sharedFileName.lastIndexOf('.')) + sharedFileName = sharedFileName.substring(0, sharedFileName.lastIndexOf('.')) + sharedFileName = sharedFileName + 'd' + sharedFileExtension + def newSharedFile = new File(sharedFileDir, sharedFileName) + + def sharedLinkFileDir = it.sharedLibraryLinkFile.parentFile + def sharedLinkFileName = it.sharedLibraryLinkFile.name + def sharedLinkFileExtension = sharedLinkFileName.substring(sharedLinkFileName.lastIndexOf('.')) + sharedLinkFileName = sharedLinkFileName.substring(0, sharedLinkFileName.lastIndexOf('.')) + sharedLinkFileName = sharedLinkFileName + 'd' + sharedLinkFileExtension + def newLinkFile = new File(sharedLinkFileDir, sharedLinkFileName) + + it.sharedLibraryLinkFile = newLinkFile + it.sharedLibraryFile = newSharedFile + } + } +} + +ext.createComponentZipTasks = { components, names, base, type, project, func -> + def stringNames = names.collect {it.toString()} + def configMap = [:] + components.each { + if (it in NativeLibrarySpec && stringNames.contains(it.name)) { + it.binaries.each { + if (!it.buildable) return + def target = nativeUtils.getPublishClassifier(it) + if (configMap.containsKey(target)) { + configMap.get(target).add(it) + } else { + configMap.put(target, []) + configMap.get(target).add(it) + } + } + } + } + def taskList = [] + def outputsFolder = file("$project.buildDir/outputs") + configMap.each { key, value -> + def task = project.tasks.create(base + "-${key}", type) { + description = 'Creates component archive for platform ' + key + destinationDirectory = outputsFolder + classifier = key + archiveBaseName = '_M_' + base + duplicatesStrategy = 'exclude' + + from(licenseFile) { + into '/' + } + + func(it, value) + } + taskList.add(task) + + project.build.dependsOn task + + project.artifacts { + task + } + addTaskToCopyAllOutputs(task) + } + return taskList +} + +ext.createAllCombined = { list, name, base, type, project -> + def outputsFolder = file("$project.buildDir/outputs") + + def task = project.tasks.create(base + "-all", type) { + description = "Creates component archive for all classifiers" + destinationDirectory = outputsFolder + classifier = "all" + archiveBaseName = base + duplicatesStrategy = 'exclude' + + list.each { + if (it.name.endsWith('debug')) return + from project.zipTree(it.archivePath) + dependsOn it + } + } + + project.build.dependsOn task + + project.artifacts { + task + } + + return task + +} + +ext.includeStandardZipFormat = { task, value -> + value.each { binary -> + if (binary.buildable) { + if (binary instanceof SharedLibraryBinarySpec) { + task.dependsOn binary.tasks.link + task.from(new File(binary.sharedLibraryFile.absolutePath + ".debug")) { + into nativeUtils.getPlatformPath(binary) + '/shared' + } + def sharedPath = binary.sharedLibraryFile.absolutePath + sharedPath = sharedPath.substring(0, sharedPath.length() - 4) + + task.from(new File(sharedPath + '.pdb')) { + into nativeUtils.getPlatformPath(binary) + '/shared' + } + task.from(binary.sharedLibraryFile) { + into nativeUtils.getPlatformPath(binary) + '/shared' + } + task.from(binary.sharedLibraryLinkFile) { + into nativeUtils.getPlatformPath(binary) + '/shared' + } + } else if (binary instanceof StaticLibraryBinarySpec) { + task.dependsOn binary.tasks.createStaticLib + task.from(binary.staticLibraryFile) { + into nativeUtils.getPlatformPath(binary) + '/static' + } + } + } + } +} diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle new file mode 100644 index 000000000..7bd75031d --- /dev/null +++ b/photon-lib/publish.gradle @@ -0,0 +1,191 @@ +apply plugin: 'maven-publish' + +ext.licenseFile = files("$rootDir/LICENSE.txt") + +def outputsFolder = file("$buildDir/outputs") +def allOutputsFolder = file("$buildDir/allOutputs") + +def versionFile = file("$allOutputsFolder/version.txt") + +task outputVersions() { + description = 'Prints the versions of wpilib to a file for use by the downstream packaging project' + group = 'Build' + outputs.files(versionFile) + + doFirst { + buildDir.mkdir() + outputsFolder.mkdir() + allOutputsFolder.mkdir() + } + + doLast { + versionFile.write pubVersion + } +} + +task libraryBuild() {} + +build.dependsOn outputVersions + +task copyAllOutputs(type: Copy) { + destinationDir allOutputsFolder +} + +build.dependsOn copyAllOutputs +copyAllOutputs.dependsOn outputVersions + +ext.addTaskToCopyAllOutputs = { task -> + copyAllOutputs.dependsOn task + copyAllOutputs.inputs.file task.archivePath + copyAllOutputs.from task.archivePath +} + +def artifactGroupId = 'org.photonvision' +def baseArtifactId = 'PhotonLib' +def zipBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-cpp_CLS" +def javaBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-java_CLS" + +task cppHeadersZip(type: Zip) { + destinationDirectory = outputsFolder + archiveBaseName = zipBaseName + classifier = "headers" + + from(licenseFile) { + into '/' + } + + from('src/main/native/include/') { + into '/' + } +} + +task cppSourceZip(type: Zip) { + destinationDirectory = outputsFolder + archiveBaseName = zipBaseName + classifier = "source" + + from(licenseFile) { + into '/' + } + + from('src/main/native/cpp') { + into '/' + } +} + +build.dependsOn cppHeadersZip +addTaskToCopyAllOutputs(cppHeadersZip) +build.dependsOn cppSourceZip +addTaskToCopyAllOutputs(cppSourceZip) + +task sourcesJar(type: Jar, dependsOn: classes) { + classifier = 'sources' + from sourceSets.main.allSource +} + +task javadocJar(type: Jar, dependsOn: javadoc) { + classifier = 'javadoc' + from javadoc.destinationDir +} + +task outputJar(type: Jar, dependsOn: classes) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + from sourceSets.main.output +} + +task outputSourcesJar(type: Jar, dependsOn: classes) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + classifier = 'sources' + from sourceSets.main.allSource +} + +task outputJavadocJar(type: Jar, dependsOn: javadoc) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + classifier = 'javadoc' + from javadoc.destinationDir +} + +def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) + +artifacts { + archives sourcesJar + archives javadocJar + archives outputJar + archives outputSourcesJar + archives outputJavadocJar +} + +addTaskToCopyAllOutputs(outputSourcesJar) +addTaskToCopyAllOutputs(outputJavadocJar) +addTaskToCopyAllOutputs(outputJar) + +build.dependsOn outputSourcesJar +build.dependsOn outputJavadocJar +build.dependsOn outputJar + +libraryBuild.dependsOn build + +def releasesRepoUrl = "$buildDir/repos/releases" + +publishing { + repositories { + maven { + url = releasesRepoUrl + } + maven { + url 'https://maven.photonvision.org/repository/internal' + credentials { + username 'ghactions' + password System.getenv("ARTIFACTORY_API_KEY") + } + } + } +} + +task cleanReleaseRepo(type: Delete) { + delete releasesRepoUrl +} + +tasks.matching {it != cleanReleaseRepo}.all {it.dependsOn cleanReleaseRepo} + +model { + publishing { + def taskList = createComponentZipTasks($.components, ['Photon'], zipBaseName, Zip, project, includeStandardZipFormat) + + publications { + cpp(MavenPublication) { + taskList.each { + artifact it + } + artifact cppHeadersZip + artifact cppSourceZip + + artifactId = "${baseArtifactId}-cpp" + groupId artifactGroupId + version pubVersion + } + java(MavenPublication) { + artifact jar + artifact sourcesJar + artifact javadocJar + + artifactId = "${baseArtifactId}-java" + groupId artifactGroupId + version pubVersion + } + vendorjson(MavenPublication) { + artifact vendorJson + + artifactId = "${baseArtifactId}-json" + groupId = artifactGroupId + version "1.0" + } + } + } +} + +publishToMavenLocal.dependsOn libraryBuild +publish.dependsOn libraryBuild diff --git a/photon-lib/settings.gradle b/photon-lib/settings.gradle new file mode 100644 index 000000000..212a8d608 --- /dev/null +++ b/photon-lib/settings.gradle @@ -0,0 +1,8 @@ +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + } +} + +rootProject.name = 'photon-lib' diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in new file mode 100644 index 000000000..e21397477 --- /dev/null +++ b/photon-lib/src/generate/photonlib.json.in @@ -0,0 +1,40 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "${photon_version}", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/lib/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision.lib", + "artifactId": "PhotonLib-cpp", + "version": "${photon_version}", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "${photon_version}" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "${photon_version}" + } + ] +} diff --git a/photon-lib/src/main/driver/cpp/VendorJNI.cpp b/photon-lib/src/main/driver/cpp/VendorJNI.cpp new file mode 100644 index 000000000..1f2833e48 --- /dev/null +++ b/photon-lib/src/main/driver/cpp/VendorJNI.cpp @@ -0,0 +1,46 @@ +/* + * 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 . + */ + +#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/cpp/driversource.cpp b/photon-lib/src/main/driver/cpp/driversource.cpp new file mode 100644 index 000000000..a9e7f69df --- /dev/null +++ b/photon-lib/src/main/driver/cpp/driversource.cpp @@ -0,0 +1,22 @@ +/* + * 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 . + */ + +#include "driverheader.h" + +extern "C" { +void c_doThing(void) {} +} // extern "C" diff --git a/photon-lib/src/main/driver/include/driverheader.h b/photon-lib/src/main/driver/include/driverheader.h new file mode 100644 index 000000000..4b706229f --- /dev/null +++ b/photon-lib/src/main/driver/include/driverheader.h @@ -0,0 +1,22 @@ +/* + * 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 + +extern "C" { +void c_doThing(void); +} // extern "C" diff --git a/photon-lib/src/main/driver/symbols.txt b/photon-lib/src/main/driver/symbols.txt new file mode 100644 index 000000000..2f9b64148 --- /dev/null +++ b/photon-lib/src/main/driver/symbols.txt @@ -0,0 +1,4 @@ +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 new file mode 100644 index 000000000..2557b06c8 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -0,0 +1,198 @@ +/* + * 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; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; + +/** Represents a camera that is connected to PhotonVision. */ +public class PhotonCamera { + final NetworkTableEntry rawBytesEntry; + final NetworkTableEntry driverModeEntry; + final NetworkTableEntry inputSaveImgEntry; + final NetworkTableEntry outputSaveImgEntry; + final NetworkTableEntry pipelineIndexEntry; + final NetworkTableEntry ledModeEntry; + + final NetworkTable mainTable = NetworkTableInstance.getDefault().getTable("photonvision"); + + boolean driverMode; + int pipelineIndex; + VisionLEDMode mode; + + Packet packet = new Packet(1); + + /** + * Constructs a PhotonCamera from a root table. + * + * @param rootTable The root table that the camera is broadcasting information over. + */ + public PhotonCamera(NetworkTable rootTable) { + rawBytesEntry = rootTable.getEntry("rawBytes"); + driverModeEntry = rootTable.getEntry("driverMode"); + inputSaveImgEntry = rootTable.getEntry("inputSaveImgCmd"); + outputSaveImgEntry = rootTable.getEntry("outputSaveImgCmd"); + pipelineIndexEntry = rootTable.getEntry("pipelineIndex"); + ledModeEntry = mainTable.getEntry("ledMode"); + + driverMode = driverModeEntry.getBoolean(false); + pipelineIndex = pipelineIndexEntry.getNumber(0).intValue(); + getLEDMode(); + } + + /** + * Constructs a PhotonCamera from the name of the camera. + * + * @param cameraName The nickname of the camera (found in the PhotonVision UI). + */ + public PhotonCamera(String cameraName) { + this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName)); + } + + /** + * Returns the latest pipeline result. + * + * @return The latest pipeline result. + */ + public PhotonPipelineResult getLatestResult() { + // Clear the packet. + packet.clear(); + + // Create latest result. + var ret = new PhotonPipelineResult(); + + // Populate packet and create result. + packet.setData(rawBytesEntry.getRaw(new byte[] {})); + if (packet.getSize() < 1) return ret; + ret.createFromPacket(packet); + + // Return result. + return ret; + } + + /** + * Returns whether the camera is in driver mode. + * + * @return Whether the camera is in driver mode. + */ + public boolean getDriverMode() { + return driverMode; + } + + /** + * Toggles driver mode. + * + * @param driverMode Whether to set driver mode. + */ + public void setDriverMode(boolean driverMode) { + if (this.driverMode != driverMode) { + this.driverMode = driverMode; + driverModeEntry.setBoolean(this.driverMode); + } + } + + /** + * Request the camera to save a new image file from the input camera stream with overlays. Images + * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk + * space and eventually cause the system to stop working. Clear out images in + * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. + */ + public void takeInputSnapshot() { + inputSaveImgEntry.setBoolean(true); + } + + /** + * Request the camera to save a new image file from the output stream with overlays. Images take + * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space + * and eventually cause the system to stop working. Clear out images in + * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. + */ + public void takeOutputSnapshot() { + outputSaveImgEntry.setBoolean(true); + } + + /** + * Returns the active pipeline index. + * + * @return The active pipeline index. + */ + public int getPipelineIndex() { + return pipelineIndex; + } + + /** + * Allows the user to select the active pipeline index. + * + * @param index The active pipeline index. + */ + public void setPipelineIndex(int index) { + if (pipelineIndex != index) { + pipelineIndex = index; + pipelineIndexEntry.setNumber(pipelineIndex); + } + } + + /** + * Returns the current LED mode. + * + * @return The current LED mode. + */ + public VisionLEDMode getLEDMode() { + int value = ledModeEntry.getNumber(-1).intValue(); + switch (value) { + case 0: + mode = VisionLEDMode.kOff; + break; + case 1: + mode = VisionLEDMode.kOn; + break; + case 2: + mode = VisionLEDMode.kBlink; + break; + case -1: + default: + mode = VisionLEDMode.kDefault; + break; + } + return mode; + } + + /** + * Sets the LED mode. + * + * @param led The mode to set to. + */ + public void setLED(VisionLEDMode led) { + if (led != mode) { + ledModeEntry.setNumber(led.value); + } + } + + /** + * Returns whether the latest target result has targets. + * + * @return Whether the latest target result has targets. + */ + public boolean hasTargets() { + return getLatestResult().hasTargets(); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java new file mode 100644 index 000000000..d1b5182f2 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java @@ -0,0 +1,166 @@ +/* + * 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; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +public final class PhotonUtils { + private PhotonUtils() { + // Utility class + } + + /** + * Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates + * range to a target using the target's elevation. This method can produce more stable results + * than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method + * requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and + * for there to exist a height differential between goal and camera. The larger this differential, + * the more accurate the distance estimate will be. + * + *

Units can be converted using the {@link edu.wpi.first.wpilibj.util.Units} class. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param targetHeightMeters The physical height of the target off the floor in meters. This + * should be the height of whatever is being targeted (i.e. if the targeting region is set to + * top, this should be the height of the top of the target). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @return The estimated distance to the target in meters. + */ + public static double calculateDistanceToTargetMeters( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians) { + return (targetHeightMeters - cameraHeightMeters) + / Math.tan(cameraPitchRadians + targetPitchRadians); + } + + /** + * Estimate the {@link Translation2d} of the target relative to the camera. + * + * @param targetDistanceMeters The distance to the target in meters. + * @param yaw The observed yaw of the target. + * @return The target's camera-relative translation. + */ + public static Translation2d estimateCameraToTargetTranslation( + double targetDistanceMeters, Rotation2d yaw) { + return new Translation2d( + yaw.getCos() * targetDistanceMeters, yaw.getSin() * targetDistanceMeters); + } + + /** + * Estimate the position of the robot in the field. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param targetHeightMeters The physical height of the target off the floor in meters. This + * should be the height of whatever is being targeted (i.e. if the targeting region is set to + * top, this should be the height of the top of the target). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and + * Photon returns CW-positive. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param cameraToRobot The position of the robot relative to the camera. If the camera was + * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be + * Transform2d(3 inches, 0 inches, 0 degrees). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians, + Rotation2d targetYaw, + Rotation2d gyroAngle, + Pose2d fieldToTarget, + Transform2d cameraToRobot) { + return PhotonUtils.estimateFieldToRobot( + PhotonUtils.estimateCameraToTarget( + PhotonUtils.estimateCameraToTargetTranslation( + PhotonUtils.calculateDistanceToTargetMeters( + cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + } + + /** + * Estimates a {@link Transform2d} that maps the camera position to the target position, using the + * robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system + * -- that is, it should read zero degrees when pointed towards the opposing alliance station, and + * increase as the robot rotates CCW. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target + * relative to the camera. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @return A Transform2d that takes us from the camera to the target. + */ + public static Transform2d estimateCameraToTarget( + Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) { + // This pose maps our camera at the origin out to our target, in the robot + // reference frame + // The translation part of this Transform2d is from the above step, and the + // rotation uses our robot's + // gyro. + return new Transform2d( + cameraToTargetTranslation, gyroAngle.times(-1).minus(fieldToTarget.getRotation())); + } + + /** + * Estimates the pose of the robot in the field coordinate system, given the position of the + * target relative to the camera, the target relative to the field, and the robot relative to the + * camera. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param cameraToRobot The position of the robot relative to the camera. If the camera was + * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be + * Transform2d(3 inches, 0 inches, 0 degrees). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) { + return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot); + } + + /** + * Estimates the pose of the camera in the field coordinate system, given the position of the + * target relative to the camera, and the target relative to the field. This *only* tracks the + * position of the camera, not the position of the robot itself. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ + public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) { + var targetToCamera = cameraToTarget.inverse(); + return fieldToTarget.transformBy(targetToCamera); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java new file mode 100644 index 000000000..29bdf855e --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java @@ -0,0 +1,70 @@ +/* + * 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; + +import edu.wpi.first.networktables.NetworkTable; +import java.util.Arrays; +import java.util.List; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class SimPhotonCamera extends PhotonCamera { + /** + * Constructs a Simulated PhotonCamera from a root table. + * + * @param rootTable The root table that the camera is broadcasting information over. + */ + public SimPhotonCamera(NetworkTable rootTable) { + super(rootTable); + } + + /** + * Constructs a Simulated PhotonCamera from the name of the camera. + * + * @param cameraName The nickname of the camera (found in the PhotonVision UI). + */ + public SimPhotonCamera(String cameraName) { + super(cameraName); + } + + /** + * Simulate one processed frame of vision data, putting one result to NT. + * + * @param latencyMillis + * @param targets Each target detected + */ + public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) { + submitProcessedFrame(latencyMillis, Arrays.asList(targets)); + } + + /** + * Simulate one processed frame of vision data, putting one result to NT. + * + * @param latencyMillis + * @param tgtList List of targets detected + */ + public void submitProcessedFrame(double latencyMillis, List tgtList) { + if (!getDriverMode()) { + PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, tgtList); + var newPacket = new Packet(newResult.getPacketSize()); + newResult.populatePacket(newPacket); + rawBytesEntry.setRaw(newPacket.getData()); + } + } +} diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java new file mode 100644 index 000000000..5e85483f7 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -0,0 +1,181 @@ +/* + * 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; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.util.Units; +import java.util.ArrayList; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class SimVisionSystem { + SimPhotonCamera cam; + + double camDiagFOVDegrees; + double camHorizFOVDegrees; + double camVertFOVDegrees; + double cameraHeightOffGroundMeters; + double maxLEDRangeMeters; + double camPitchDegrees; + int cameraResWidth; + int cameraResHeight; + double minTargetArea; + Transform2d cameraToRobot; + + ArrayList tgtList; + + /** + * Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot + * running Photonvision, detecting one or more targets scattered around the field. This assumes a + * fairly simple and distortionless pinhole camera model. + * + * @param camName Name of the photonvision camera to create. Align it with the settings you use in + * the PhotonVision GUI. + * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the + * manufacturer specifications, and/or whatever is configured in the PhotonVision Setting + * page. + * @param camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same + * as whatever is configured in the PhotonVision Setting page. + * @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's + * position + * @param cameraHeightOffGroundMeters Height of the camera off the ground in meters + * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and + * make it visible. Set to 9000 or more if your vision system does not rely on LED's. + * @param cameraResWidth Width of your camera's image sensor in pixels + * @param cameraResHeight Height of your camera's image sensor in pixels + * @param minTargetArea Minimum area that that the target should be before it's recognized as a + * target by the camera. Match this with your contour filtering settings in the PhotonVision + * GUI. + */ + public SimVisionSystem( + String camName, + double camDiagFOVDegrees, + double camPitchDegrees, + Transform2d cameraToRobot, + double cameraHeightOffGroundMeters, + double maxLEDRangeMeters, + int cameraResWidth, + int cameraResHeight, + double minTargetArea) { + this.camDiagFOVDegrees = camDiagFOVDegrees; + this.camPitchDegrees = camPitchDegrees; + this.cameraToRobot = cameraToRobot; + this.cameraHeightOffGroundMeters = cameraHeightOffGroundMeters; + this.maxLEDRangeMeters = maxLEDRangeMeters; + this.cameraResWidth = cameraResWidth; + this.cameraResHeight = cameraResHeight; + this.minTargetArea = minTargetArea; + + // Calculate horizontal/vertical FOV by similar triangles + double hypotPixels = Math.hypot(cameraResWidth, cameraResHeight); + this.camHorizFOVDegrees = camDiagFOVDegrees * cameraResWidth / hypotPixels; + this.camVertFOVDegrees = camDiagFOVDegrees * cameraResHeight / hypotPixels; + + cam = new SimPhotonCamera(camName); + tgtList = new ArrayList(); + } + + /** + * Add a target on the field which your vision system is designed to detect. The photoncamera from + * this system will report the location of the robot relative to the subste of these targets which + * are visible from the given robot position. + * + * @param tgt + */ + public void addSimVisionTarget(SimVisionTarget tgt) { + tgtList.add(tgt); + } + + /** + * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or + * turret or some other mobile platform. + * + * @param newCameraToRobot New Tranform from the robot to the camera + * @param newCamHeightMeters New height of the camera off the floor + * @param newCamPitchDegrees New pitch of the camera axis back from horizontal + */ + public void moveCamera( + Transform2d newCameraToRobot, double newCamHeightMeters, double newCamPitchDegrees) { + this.cameraToRobot = newCameraToRobot; + this.cameraHeightOffGroundMeters = newCamHeightMeters; + this.camPitchDegrees = newCamPitchDegrees; + } + + /** + * Periodic update. Call this once per frame of image data you wish to process and send to + * NetworkTables + * + * @param robotPoseMeters current pose of the robot on the field. Will be used to calcualte which + * targets are actually in view, where they are at relative to the robot, and relevant + * PhotonVision parameters. + */ + public void processFrame(Pose2d robotPoseMeters) { + + Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse()); + + ArrayList visibleTgtList = new ArrayList<>(tgtList.size()); + + tgtList.forEach( + (tgt) -> { + var camToTargetTrans = new Transform2d(cameraPos, tgt.targetPos); + + double distAlongGroundMeters = camToTargetTrans.getTranslation().getNorm(); + double distVerticalMeters = + tgt.targetHeightAboveGroundMeters - this.cameraHeightOffGroundMeters; + double distMeters = Math.hypot(distAlongGroundMeters, distVerticalMeters); + + double area = tgt.tgtAreaMeters2 / getM2PerPx(distAlongGroundMeters); + + // 2D yaw mode considers the target as a point, and should ignore target rotation. + // Photon reports it in the correct robot reference frame. + // IE: targets to the left of the image should report negative yaw. + double yawDegrees = + -1.0 + * Units.radiansToDegrees( + Math.atan2( + camToTargetTrans.getTranslation().getY(), + camToTargetTrans.getTranslation().getX())); + double pitchDegrees = + Units.radiansToDegrees(Math.atan2(distVerticalMeters, distAlongGroundMeters)) + - this.camPitchDegrees; + + if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) { + visibleTgtList.add( + new PhotonTrackedTarget(yawDegrees, pitchDegrees, area, 0.0, camToTargetTrans)); + } + }); + + cam.submitProcessedFrame(0.0, visibleTgtList); + } + + double getM2PerPx(double dist) { + double widthMPerPx = + 2 * dist * Math.tan(Units.degreesToRadians(this.camHorizFOVDegrees) / 2) / cameraResWidth; + double heightMPerPx = + 2 * dist * Math.tan(Units.degreesToRadians(this.camVertFOVDegrees) / 2) / cameraResHeight; + return widthMPerPx * heightMPerPx; + } + + boolean camCanSeeTarget(double distMeters, double yaw, double pitch, double area) { + boolean inRange = (distMeters < this.maxLEDRangeMeters); + boolean inHorizAngle = Math.abs(yaw) < (this.camHorizFOVDegrees / 2); + boolean inVertAngle = Math.abs(pitch) < (this.camVertFOVDegrees / 2); + boolean targetBigEnough = area > this.minTargetArea; + return (inRange && inHorizAngle && inVertAngle && targetBigEnough); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java new file mode 100644 index 000000000..60b896d02 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java @@ -0,0 +1,51 @@ +/* + * 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; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +public class SimVisionTarget { + Pose2d targetPos; + double targetWidthMeters; + double targetHeightMeters; + double targetHeightAboveGroundMeters; + double targetInfill_pct; + double tgtAreaMeters2; + + /** + * Describes a vision target located somewhere on the field that your SimVisionSystem can detect. + * + * @param targetPos Pose2d of the target on the field. Define it such that, if you are standing on + * the middle of the field facing the target, the Y axis points to your left, and the X axis + * points away from you. + * @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters. + * @param targetWidthMeters Width of the outter bounding box of the target in meters. + * @param targetHeightMeters Pair Height of the outter bounding box of the target in meters. + */ + public SimVisionTarget( + Pose2d targetPos, + double targetHeightAboveGroundMeters, + double targetWidthMeters, + double targetHeightMeters) { + this.targetPos = targetPos; + this.targetHeightAboveGroundMeters = targetHeightAboveGroundMeters; + this.targetWidthMeters = targetWidthMeters; + this.targetHeightMeters = targetHeightMeters; + this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters; + } +} diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp new file mode 100644 index 000000000..b0a0ce472 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -0,0 +1,90 @@ +/* + * 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 . + */ + +#include "photonlib/PhotonCamera.h" + +#include "photonlib/Packet.h" + +namespace photonlib { +PhotonCamera::PhotonCamera(std::shared_ptr rootTable) + : rawBytesEntry(rootTable->GetEntry("rawBytes")), + driverModeEntry(rootTable->GetEntry("driverMode")), + inputSaveImgEntry(rootTable->GetEntry("inputSaveImgCmd")), + outputSaveImgEntry(rootTable->GetEntry("outputSaveImgCmd")), + pipelineIndexEntry(rootTable->GetEntry("pipelineIndex")), + ledModeEntry(mainTable->GetEntry("ledMode")) { + driverMode = driverModeEntry.GetBoolean(false); + pipelineIndex = static_cast(pipelineIndexEntry.GetDouble(0.0)); + mode = GetLEDMode(); +} + +PhotonCamera::PhotonCamera(const std::string& cameraName) + : PhotonCamera(nt::NetworkTableInstance::GetDefault() + .GetTable("photonvision") + ->GetSubTable(cameraName)) {} + +PhotonPipelineResult PhotonCamera::GetLatestResult() const { + // Clear the current packet. + packet.Clear(); + + // Create the new result; + PhotonPipelineResult result; + + // Fill the packet with latest data and populate result. + std::string value = rawBytesEntry.GetValue()->GetRaw(); + std::vector bytes{value.begin(), value.end()}; + + photonlib::Packet packet{bytes}; + + packet >> result; + return result; +} + +void PhotonCamera::SetDriverMode(bool driverMode) { + if (this->driverMode != driverMode) { + this->driverMode = driverMode; + driverModeEntry.SetBoolean(this->driverMode); + } +} + +void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.SetBoolean(true); } + +void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.SetBoolean(true); } + +bool PhotonCamera::GetDriverMode() const { return driverMode; } + +void PhotonCamera::SetPipelineIndex(int index) { + if (index != pipelineIndex) { + pipelineIndex = index; + pipelineIndexEntry.SetDouble(static_cast(pipelineIndex)); + } +} + +int PhotonCamera::GetPipelineIndex() const { return pipelineIndex; } + +LEDMode PhotonCamera::GetLEDMode() const { + mode = static_cast(static_cast(ledModeEntry.GetDouble(-1.0))); + return mode; +} + +void PhotonCamera::SetLEDMode(LEDMode led) { + if (led != mode) { + mode = led; + ledModeEntry.SetDouble(static_cast(static_cast(mode))); + } +} +} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp new file mode 100644 index 000000000..69354da9f --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -0,0 +1,67 @@ +/* + * 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 . + */ + +#include "photonlib/PhotonPipelineResult.h" + +namespace photonlib { +PhotonPipelineResult::PhotonPipelineResult( + units::second_t latency, wpi::ArrayRef targets) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()) { + hasTargets = targets.size() != 0; +} + +bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { + return latency == other.latency && hasTargets == other.hasTargets && + targets == other.targets; +} + +bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const { + return !operator==(other); +} + +Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { + // Encode latency, existence of targets, and number of targets. + packet << result.latency.to() * 1000 << result.hasTargets + << static_cast(result.targets.size()); + + // Encode the information of each target. + for (auto& target : result.targets) packet << target; + + // Return the packet + return packet; +} + +Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { + // Decode latency, existence of targets, and number of targets. + int8_t targetCount = 0; + double latencyMillis = 0; + packet >> latencyMillis >> result.hasTargets >> targetCount; + result.latency = units::second_t(latencyMillis / 1000.0); + + result.targets.clear(); + + // Decode the information of each target. + for (int i = 0; i < targetCount; ++i) { + PhotonTrackedTarget target; + packet >> target; + result.targets.push_back(target); + } + return packet; +} + +} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp new file mode 100644 index 000000000..d5005e747 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -0,0 +1,60 @@ +/* + * 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 . + */ + +#include "photonlib/PhotonTrackedTarget.h" + +#include + +#include + +namespace photonlib { + +PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area, + double skew, + const frc::Transform2d& pose) + : yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {} + +bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { + return other.yaw == yaw && other.pitch == pitch && other.area == area && + other.skew == skew && other.cameraToTarget == cameraToTarget; +} + +bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { + return !operator==(other); +} + +Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { + return packet << target.yaw << target.pitch << target.area << target.skew + << target.cameraToTarget.Translation().X().to() + << target.cameraToTarget.Translation().Y().to() + << target.cameraToTarget.Rotation().Degrees().to(); +} + +Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { + packet >> target.yaw >> target.pitch >> target.area >> target.skew; + double x = 0; + double y = 0; + double rot = 0; + packet >> x >> y >> rot; + + target.cameraToTarget = + frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)), + units::degree_t(rot)); + return packet; +} + +} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp new file mode 100644 index 000000000..ce8f4e2c6 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#include "photonlib/SimPhotonCamera.h" + +namespace photonlib { + +SimPhotonCamera::SimPhotonCamera(std::shared_ptr rootTable) + : PhotonCamera(rootTable) {} + +SimPhotonCamera::SimPhotonCamera(const std::string& cameraName) + : PhotonCamera(cameraName) {} + +void SimPhotonCamera::SubmitProcessedFrame( + units::second_t latency, wpi::ArrayRef tgtList) { + if (!GetDriverMode()) { + // Clear the current packet. + simPacket.Clear(); + + // Create the new result and pump it into the packet + simPacket << PhotonPipelineResult(latency, tgtList); + + rawBytesEntry.SetRaw( + wpi::StringRef(simPacket.GetData().data(), simPacket.GetData().size())); + } +} + +} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp b/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp new file mode 100644 index 000000000..971dc6fb0 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp @@ -0,0 +1,119 @@ +/* + * 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 . + */ + +#include "photonlib/SimVisionSystem.h" + +#include + +#include +#include + +namespace photonlib { + +SimVisionSystem::SimVisionSystem(const std::string& name, + units::degree_t camDiagFOV, + units::degree_t camPitch, + frc::Transform2d cameraToRobot, + units::meter_t cameraHeightOffGround, + units::meter_t maxLEDRange, int cameraResWidth, + int cameraResHeight, double minTargetArea) + : camDiagFOV(camDiagFOV), + camPitch(camPitch), + cameraToRobot(cameraToRobot), + cameraHeightOffGround(cameraHeightOffGround), + maxLEDRange(maxLEDRange), + cameraResWidth(cameraResWidth), + cameraResHeight(cameraResHeight), + minTargetArea(minTargetArea) { + double hypotPixels = std::hypot(cameraResWidth, cameraResHeight); + camHorizFOV = camDiagFOV * cameraResWidth / hypotPixels; + camVertFOV = camDiagFOV * cameraResHeight / hypotPixels; + + cam = SimPhotonCamera(name); + tgtList.clear(); +} + +void SimVisionSystem::AddSimVisionTarget(SimVisionTarget tgt) { + tgtList.push_back(tgt); +} + +void SimVisionSystem::MoveCamera(frc::Transform2d newCameraToRobot, + units::meter_t newCamHeight, + units::degree_t newCamPitch) { + cameraToRobot = newCameraToRobot; + cameraHeightOffGround = newCamHeight; + camPitch = newCamPitch; +} + +void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) { + frc::Pose2d cameraPos = robotPose.TransformBy(cameraToRobot.Inverse()); + std::vector visibleTgtList = {}; + + for (auto&& tgt : tgtList) { + frc::Transform2d camToTargetTrans = + frc::Transform2d(cameraPos, tgt.targetPos); + + units::meter_t distAlongGround = camToTargetTrans.Translation().Norm(); + units::meter_t distVertical = + tgt.targetHeightAboveGround - cameraHeightOffGround; + units::meter_t distHypot = + units::math::hypot(distAlongGround, distVertical); + + double area = tgt.tgtArea.to() / GetM2PerPx(distAlongGround); + + // 2D yaw mode considers the target as a point, and should ignore target + // rotation. + // Photon reports it in the correct robot reference frame. + // IE: targets to the left of the image should report negative yaw. + units::degree_t yawAngle = + -1.0 * units::math::atan2(camToTargetTrans.Translation().Y(), + camToTargetTrans.Translation().X()); + units::degree_t pitchAngle = + units::math::atan2(distVertical, distAlongGround) - camPitch; + + if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) { + PhotonTrackedTarget newTgt = + PhotonTrackedTarget(yawAngle.to(), pitchAngle.to(), + area, 0.0, camToTargetTrans); + visibleTgtList.push_back(newTgt); + } + } + + units::second_t procDelay(0.0); // Future - tie this to something meaningful + cam.SubmitProcessedFrame( + procDelay, wpi::MutableArrayRef(visibleTgtList)); +} + +double SimVisionSystem::GetM2PerPx(units::meter_t dist) { + double heightMPerPx = 2 * dist.to() * + units::math::tan(camVertFOV / 2) / cameraResHeight; + double widthMPerPx = 2 * dist.to() * + units::math::tan(camHorizFOV / 2) / cameraResWidth; + return widthMPerPx * heightMPerPx; +} + +bool SimVisionSystem::CamCanSeeTarget(units::meter_t distHypot, + units::degree_t yaw, + units::degree_t pitch, double area) { + bool inRange = (distHypot < maxLEDRange); + bool inHorizAngle = units::math::abs(yaw) < (camHorizFOV / 2); + bool inVertAngle = units::math::abs(pitch) < (camVertFOV / 2); + bool targetBigEnough = area > minTargetArea; + return (inRange && inHorizAngle && inVertAngle && targetBigEnough); +} + +} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp new file mode 100644 index 000000000..13620843d --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp @@ -0,0 +1,33 @@ +/* + * 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 . + */ + +#include "photonlib/SimVisionTarget.h" + +namespace photonlib { + +SimVisionTarget::SimVisionTarget(frc::Pose2d& targetPos, + units::meter_t targetHeightAboveGround, + units::meter_t targetWidth, + units::meter_t targetHeight) + : targetPos(targetPos), + targetHeightAboveGround(targetHeightAboveGround), + targetWidth(targetWidth), + targetHeight(targetHeight) { + tgtArea = targetWidth * targetHeight; +} + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/Packet.h b/photon-lib/src/main/native/include/photonlib/Packet.h new file mode 100644 index 000000000..03bf55d20 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/Packet.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 + +namespace photonlib { + +/** + * A packet that holds byte-packed data to be sent over NetworkTables. + */ +class Packet { + public: + /** + * Constructs an empty packet. + */ + Packet() = default; + + /** + * Constructs a packet with the given data. + * @param data The packet data. + */ + explicit Packet(std::vector data) : packetData(data) {} + + /** + * Clears the packet and resets the read and write positions. + */ + void Clear() { + packetData.clear(); + readPos = 0; + writePos = 0; + } + + /** + * Returns the packet data. + * @return The packet data. + */ + const std::vector& GetData() { return packetData; } + + /** + * Returns the number of bytes in the data. + * @return The number of bytes in the data. + */ + size_t GetDataSize() const { return packetData.size(); } + + /** + * Adds a value to the data buffer. This should only be used with PODs. + * @tparam T The data type. + * @param src The data source. + * @return A reference to the current object. + */ + template + Packet& operator<<(T src) { + packetData.resize(packetData.size() + sizeof(T)); + std::memcpy(packetData.data() + writePos, &src, sizeof(T)); + + if constexpr (wpi::support::endian::system_endianness() == + wpi::support::endianness::little) { + // Reverse to big endian for network conventions. + std::reverse(packetData.data() + writePos, + packetData.data() + writePos + sizeof(T)); + } + + writePos += sizeof(T); + return *this; + } + + /** + * Extracts a value to the provided destination. + * @tparam T The type of value to extract. + * @param value The value to extract. + * @return A reference to the current object. + */ + template + Packet& operator>>(T& value) { + std::memcpy(&value, packetData.data() + readPos, sizeof(T)); + + if constexpr (wpi::support::endian::system_endianness() == + wpi::support::endianness::little) { + // Reverse to little endian for host. + char& raw = reinterpret_cast(value); + std::reverse(&raw, &raw + sizeof(T)); + } + + readPos += sizeof(T); + return *this; + } + + bool operator==(const Packet& right) const { + return packetData == right.packetData; + } + bool operator!=(const Packet& right) const { return !operator==(right); } + + private: + // Data stored in the packet + std::vector packetData; + + size_t readPos = 0; + size_t writePos = 0; +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h new file mode 100644 index 000000000..b6c87aefa --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -0,0 +1,141 @@ +/* + * 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 "photonlib/PhotonPipelineResult.h" + +namespace photonlib { + +enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 }; + +/** + * Represents a camera that is connected to PhotonVision.ß + */ +class PhotonCamera { + public: + /** + * Constructs a PhotonCamera from a root table. + * @param rootTable The root table that the camera is broadcasting information + * over. + */ + explicit PhotonCamera(std::shared_ptr rootTable); + + /** + * Constructs a PhotonCamera from the name of the camera. + * @param cameraName The nickname of the camera (found in the PhotonVision + * UI). + */ + explicit PhotonCamera(const std::string& cameraName); + + /** + * Returns the latest pipeline result. + * @return The latest pipeline result. + */ + PhotonPipelineResult GetLatestResult() const; + + /** + * Toggles driver mode. + * @param driverMode Whether to set driver mode. + */ + void SetDriverMode(bool driverMode); + + /** + * Returns whether the camera is in driver mode. + * @return Whether the camera is in driver mode. + */ + bool GetDriverMode() const; + + /** + * Request the camera to save a new image file from the input + * camera stream with overlays. + * Images take up space in the filesystem of the PhotonCamera. + * Calling it frequently will fill up disk space and eventually + * cause the system to stop working. + * Clear out images in /opt/photonvision/photonvision_config/imgSaves + * frequently to prevent issues. + */ + void TakeInputSnapshot(void); + + /** + * Request the camera to save a new image file from the output + * stream with overlays. + * Images take up space in the filesystem of the PhotonCamera. + * Calling it frequently will fill up disk space and eventually + * cause the system to stop working. + * Clear out images in /opt/photonvision/photonvision_config/imgSaves + * frequently to prevent issues. + */ + void TakeOutputSnapshot(void); + + /** + * Allows the user to select the active pipeline index. + * @param index The active pipeline index. + */ + void SetPipelineIndex(int index); + + /** + * Returns the active pipeline index. + * @return The active pipeline index. + */ + int GetPipelineIndex() const; + + /** + * Returns the current LED mode. + * @return The current LED mode. + */ + LEDMode GetLEDMode() const; + + /** + * Sets the LED mode. + * @param led The mode to set to. + */ + void SetLEDMode(LEDMode led); + + /** + * Returns whether the latest target result has targets. + * @return Whether the latest target result has targets. + */ + bool HasTargets() const { return GetLatestResult().HasTargets(); } + + private: + std::shared_ptr mainTable = + nt::NetworkTableInstance::GetDefault().GetTable("photonvision"); + + protected: + nt::NetworkTableEntry rawBytesEntry; + nt::NetworkTableEntry driverModeEntry; + nt::NetworkTableEntry inputSaveImgEntry; + nt::NetworkTableEntry outputSaveImgEntry; + nt::NetworkTableEntry pipelineIndexEntry; + nt::NetworkTableEntry ledModeEntry; + + mutable Packet packet; + + bool driverMode; + double pipelineIndex; + mutable LEDMode mode; +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h new file mode 100644 index 000000000..3a20b7d09 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.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 "photonlib/Packet.h" +#include "photonlib/PhotonTrackedTarget.h" + +namespace photonlib { +/** + * Represents a pipeline result from a PhotonCamera. + */ +class PhotonPipelineResult { + public: + /** + * Constructs an empty pipeline result. + */ + PhotonPipelineResult() = default; + + /** + * Constructs a pipeline result. + * @param latency The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + */ + PhotonPipelineResult(units::second_t latency, + wpi::ArrayRef targets); + + /** + * Returns the best target in this pipeline result. If there are no targets, + * this method will return an empty target with all values set to zero. The + * best target is determined by the target sort mode in the PhotonVision UI. + * + * @return The best target of the pipeline result. + */ + PhotonTrackedTarget GetBestTarget() const { + if (!HasTargets() && !HAS_WARNED) { + ::frc::DriverStation::ReportError( + "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"); + HAS_WARNED = true; + } + return hasTargets ? targets[0] : PhotonTrackedTarget(); + } + + /** + * Returns the latency in the pipeline. + * @return The latency in the pipeline. + */ + units::second_t GetLatency() const { return latency; } + + /** + * Returns whether the pipeline has targets. + * @return Whether the pipeline has targets. + */ + bool HasTargets() const { return hasTargets; } + + /** + * Returns a reference to the vector of targets. + * @return A reference to the vector of targets. + */ + const wpi::ArrayRef GetTargets() const { + return targets; + } + + bool operator==(const PhotonPipelineResult& other) const; + bool operator!=(const PhotonPipelineResult& other) const; + + friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); + friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); + + private: + units::second_t latency; + bool hasTargets; + wpi::SmallVector targets; + inline static bool HAS_WARNED = false; +}; +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h new file mode 100644 index 000000000..d4ddbb3b4 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -0,0 +1,93 @@ +/* + * 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 "photonlib/Packet.h" + +namespace photonlib { +/** + * Represents a tracked target within a pipeline. + */ +class PhotonTrackedTarget { + public: + /** + * Constructs an empty target. + */ + PhotonTrackedTarget() = default; + + /** + * Constructs a target. + * @param yaw The yaw of the target. + * @param pitch The pitch of the target. + * @param area The area of the target. + * @param skew The skew of the target. + * @param pose The camera-relative pose of the target. + */ + PhotonTrackedTarget(double yaw, double pitch, double area, double skew, + const frc::Transform2d& pose); + + /** + * Returns the target yaw (positive-left). + * @return The target yaw. + */ + double GetYaw() const { return yaw; } + + /** + * Returns the target pitch (positive-up) + * @return The target pitch. + */ + double GetPitch() const { return pitch; } + + /** + * Returns the target area (0-100). + * @return The target area. + */ + double GetArea() const { return area; } + + /** + * Returns the target skew (counter-clockwise positive). + * @return The target skew. + */ + double GetSkew() const { return skew; } + + /** + * Returns the pose of the target relative to the robot. + * @return The pose of the target relative to the robot. + */ + frc::Transform2d GetCameraRelativePose() const { return cameraToTarget; } + + bool operator==(const PhotonTrackedTarget& other) const; + bool operator!=(const PhotonTrackedTarget& other) const; + + friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); + friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); + + private: + double yaw = 0; + double pitch = 0; + double area = 0; + double skew = 0; + frc::Transform2d cameraToTarget; +}; +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonUtils.h b/photon-lib/src/main/native/include/photonlib/PhotonUtils.h new file mode 100644 index 000000000..bf9c4b29b --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/PhotonUtils.h @@ -0,0 +1,173 @@ +/* + * 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 photonlib { +class PhotonUtils { + public: + /** + * Algorithm from + * https://docs.limelightvision.io/en/latest/cs_estimating_distance.html + * Estimates range to a target using the target's elevation. This method can + * produce more stable results than SolvePNP when well tuned, if the full 6d + * robot pose is not required. + * + * @param cameraHeight The height of the camera off the floor. + * @param targetHeight The height of the target off the floor. + * @param cameraPitch The pitch of the camera from the horizontal plane. + * Positive valueBytes up. + * @param targetPitch The pitch of the target in the camera's lens. Positive + * values up. + * @return The estimated distance to the target. + */ + static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, + units::meter_t targetHeight, + units::radian_t cameraPitch, + units::radian_t targetPitch) { + return (targetHeight - cameraHeight) / + units::math::tan(cameraPitch + targetPitch); + } + + /** + * Estimate the Translation2d of the target relative to the camera. + * + * @param targetDistance The distance to the target. + * @param yaw The observed yaw of the target. + * + * @return The target's camera-relative translation. + */ + static frc::Translation2d EstimateCameraToTargetTranslation( + units::meter_t targetDistance, const frc::Rotation2d& yaw) { + return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()}; + } + + /** + * Estimate the position of the robot in the field. + * + * @param cameraHeightMeters The physical height of the camera off the floor + * in meters. + * @param targetHeightMeters The physical height of the target off the floor + * in meters. This should be the height of whatever is being targeted (i.e. if + * the targeting region is set to top, this should be the height of the top of + * the target). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane + * in radians. Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in + * radians. Positive values up. + * @param targetYaw The observed yaw of the target. Note that this + * *must* be CCW-positive, and Photon returns + * CW-positive. + * @param gyroAngle The current robot gyro angle, likely from + * odometry. + * @param fieldToTarget A frc::Pose2d representing the target position in + * the field coordinate system. + * @param cameraToRobot The position of the robot relative to the camera. + * If the camera was mounted 3 inches behind the + * "origin" (usually physical center) of the robot, + * this would be frc::Transform2d(3 inches, 0 + * inches, 0 degrees). + * @return The position of the robot in the field. + */ + static frc::Pose2d EstimateFieldToRobot( + units::meter_t cameraHeight, units::meter_t targetHeight, + units::radian_t cameraPitch, units::radian_t targetPitch, + const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, + const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) { + return EstimateFieldToRobot( + EstimateCameraToTarget( + EstimateCameraToTargetTranslation( + CalculateDistanceToTarget(cameraHeight, targetHeight, + cameraPitch, targetPitch), + targetYaw), + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); + } + + /** + * Estimates a {@link frc::Transform2d} that maps the camera position to the + * target position, using the robot's gyro. Note that the gyro angle provided + * *must* line up with the field coordinate system -- that is, it should read + * zero degrees when pointed towards the opposing alliance station, and + * increase as the robot rotates CCW. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y + * position of the target relative to the + * camera. + * @param fieldToTarget A frc::Pose2d representing the target + * position in the field coordinate system. + * @param gyroAngle The current robot gyro angle, likely from + * odometry. + * @return A frc::Transform2d that takes us from the camera to the target. + */ + static frc::Transform2d EstimateCameraToTarget( + const frc::Translation2d& cameraToTargetTranslation, + const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) { + // This pose maps our camera at the origin out to our target, in the robot + // reference frame + // The translation part of this frc::Transform2d is from the above step, and + // the rotation uses our robot's gyro. + return frc::Transform2d(cameraToTargetTranslation, + -gyroAngle - fieldToTarget.Rotation()); + } + + /** + * Estimates the pose of the robot in the field coordinate system, given the + * position of the target relative to the camera, the target relative to the + * field, and the robot relative to the camera. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param cameraToRobot The position of the robot relative to the camera. If + * the camera was mounted 3 inches behind the "origin" + * (usually physical center) of the robot, this would be + * frc::Transform2d(3 inches, 0 inches, 0 degrees). + * @return The position of the robot in the field. + */ + static frc::Pose2d EstimateFieldToRobot( + const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget, + const frc::Transform2d& cameraToRobot) { + return EstimateFieldToCamera(cameraToTarget, fieldToTarget) + .TransformBy(cameraToRobot); + } + + /** + * Estimates the pose of the camera in the field coordinate system, given the + * position of the target relative to the camera, and the target relative to + * the field. This *only* tracks the position of the camera, not the position + * of the robot itself. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ + static frc::Pose2d EstimateFieldToCamera( + const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget) { + auto targetToCamera = cameraToTarget.Inverse(); + return fieldToTarget.TransformBy(targetToCamera); + } +}; +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h new file mode 100644 index 000000000..1a53138b4 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -0,0 +1,65 @@ +/* + * 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 "photonlib/Packet.h" +#include "photonlib/PhotonCamera.h" + +namespace photonlib { + +/** + * Represents a camera that is connected to PhotonVision.ß + */ +class SimPhotonCamera : public PhotonCamera { + public: + /** + * Constructs a Simulated PhotonCamera from a root table. + * + * @param rootTable The root table that the camera is broadcasting information + * over. + */ + explicit SimPhotonCamera(std::shared_ptr rootTable); + + /** + * Constructs a Simulated PhotonCamera from the name of the camera. + * + * @param cameraName The nickname of the camera (found in the PhotonVision + * UI). + */ + explicit SimPhotonCamera(const std::string& cameraName); + + /** + * Simulate one processed frame of vision data, putting one result to NT. + * @param latency Latency of frame processing + * @param tgtList Set of targets detected + */ + void SubmitProcessedFrame(units::second_t latency, + wpi::ArrayRef tgtList); + + private: + mutable Packet simPacket; +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h new file mode 100644 index 000000000..419ac63f6 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h @@ -0,0 +1,74 @@ +/* + * 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 +#include +#include + +#include "photonlib/SimPhotonCamera.h" +#include "photonlib/SimVisionTarget.h" + +namespace photonlib { + +/** + * Represents a camera that is connected to PhotonVision. + */ +class SimVisionSystem { + public: + explicit SimVisionSystem(const std::string& name, units::degree_t camDiagFOV, + units::degree_t camPitch, + frc::Transform2d cameraToRobot, + units::meter_t cameraHeightOffGround, + units::meter_t maxLEDRange, int cameraResWidth, + int cameraResHeight, double minTargetArea); + + void AddSimVisionTarget(SimVisionTarget tgt); + void MoveCamera(frc::Transform2d newcameraToRobot, + units::meter_t newCamHeight, units::degree_t newCamPitch); + void ProcessFrame(frc::Pose2d robotPose); + + private: + units::degree_t camDiagFOV; + units::degree_t camPitch; + frc::Transform2d cameraToRobot; + units::meter_t cameraHeightOffGround; + units::meter_t maxLEDRange; + int cameraResWidth; + int cameraResHeight; + double minTargetArea; + units::degree_t camHorizFOV; + units::degree_t camVertFOV; + std::vector tgtList = {}; + + double GetM2PerPx(units::meter_t dist); + bool CamCanSeeTarget(units::meter_t distHypot, units::degree_t yaw, + units::degree_t pitch, double area); + + public: + SimPhotonCamera cam = photonlib::SimPhotonCamera("Default"); +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h new file mode 100644 index 000000000..3ebb66d26 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h @@ -0,0 +1,45 @@ +/* + * 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 + +namespace photonlib { + +/** + * Represents a target on the field which the vision processing system could + * detect. + */ +class SimVisionTarget { + public: + explicit SimVisionTarget(frc::Pose2d& targetPos, + units::meter_t targetHeightAboveGround, + units::meter_t targetWidth, + units::meter_t targetHeight); + + frc::Pose2d targetPos; + units::meter_t targetHeightAboveGround; + units::meter_t targetWidth; + units::meter_t targetHeight; + double targetInfill_pct; + units::square_meter_t tgtArea; +}; + +} // namespace photonlib diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java new file mode 100644 index 000000000..1574ccf72 --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -0,0 +1,98 @@ +/* + * 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; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +class PacketTest { + @Test + void testSimpleTrackedTarget() { + var target = + new PhotonTrackedTarget( + 3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))); + var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES); + target.populatePacket(p); + + var b = new PhotonTrackedTarget(); + b.createFromPacket(p); + + Assertions.assertEquals(target, b); + } + + @Test + void testSimplePipelineResult() { + var result = new PhotonPipelineResult(1, new ArrayList<>()); + var p = new Packet(result.getPacketSize()); + result.populatePacket(p); + + var b = new PhotonPipelineResult(); + b.createFromPacket(p); + + Assertions.assertEquals(result, b); + + var result2 = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5))))); + var p2 = new Packet(result2.getPacketSize()); + result2.populatePacket(p2); + + var b2 = new PhotonPipelineResult(); + b2.createFromPacket(p2); + + Assertions.assertEquals(result2, b2); + } + + @Test + void testBytePackFromCpp() { + byte[] bytePack = { + 64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0, 0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0, 0, + 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47, + 122 + }; + var t = new PhotonTrackedTarget(); + t.createFromPacket(new Packet(bytePack)); + + var target = + new PhotonTrackedTarget( + 3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))); + + Assertions.assertEquals(t, target); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java new file mode 100644 index 000000000..b405415bd --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -0,0 +1,39 @@ +/* + * 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; + +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; + +class PhotonCameraTest { + @Test + public void testEmpty() { + Assertions.assertDoesNotThrow( + () -> { + var packet = new Packet(1); + var ret = new PhotonPipelineResult(); + packet.setData(new byte[0]); + if (packet.getSize() < 1) { + return; + } + ret.createFromPacket(packet); + }); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java new file mode 100644 index 000000000..ebcc8626d --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java @@ -0,0 +1,78 @@ +/* + * 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; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.util.Units; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +class PhotonUtilTest { + @Test + public void testDistance() { + var camHeight = 1; + var targetHeight = 3; + var camPitch = Units.degreesToRadians(0); + var targetPitch = Units.degreesToRadians(30); + + var dist = + PhotonUtils.calculateDistanceToTargetMeters(camHeight, targetHeight, camPitch, targetPitch); + + Assertions.assertEquals(3.464, dist, 0.01); + + camHeight = 1; + targetHeight = 2; + camPitch = Units.degreesToRadians(20); + targetPitch = Units.degreesToRadians(-10); + + dist = + PhotonUtils.calculateDistanceToTargetMeters(camHeight, targetHeight, camPitch, targetPitch); + Assertions.assertEquals(5.671, dist, 0.01); + } + + @Test + public void testTransform() { + + var camHeight = 1; + var tgtHeight = 3; + var camPitch = 0; + var tgtPitch = Units.degreesToRadians(30); + var tgtYaw = new Rotation2d(); + var gyroAngle = new Rotation2d(); + var fieldToTarget = new Pose2d(); + var cameraToRobot = new Transform2d(); + + var fieldToRobot = + PhotonUtils.estimateFieldToRobot( + PhotonUtils.estimateCameraToTarget( + PhotonUtils.estimateCameraToTargetTranslation( + PhotonUtils.calculateDistanceToTargetMeters( + camHeight, tgtHeight, camPitch, tgtPitch), + tgtYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + + Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getY(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java new file mode 100644 index 000000000..600a2e7e4 --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java @@ -0,0 +1,307 @@ +/* + * 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; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.util.Units; +import java.util.List; +import java.util.stream.Stream; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; +import org.junit.jupiter.params.provider.ValueSource; +import org.photonvision.targeting.PhotonTrackedTarget; + +class SimVisionSystemTest { + @Test + public void testEmpty() { + Assertions.assertDoesNotThrow( + () -> { + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose2d(), 0.0, 1.0, 1.0)); + for (int loopIdx = 0; loopIdx < 100; loopIdx++) { + sysUnderTest.processFrame(new Pose2d()); + } + }); + } + + @ParameterizedTest + @ValueSource(doubles = {5, 10, 15, 20, 25, 30}) + public void testDistanceAligned(double dist) { + + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0)); + + final var robotPose = new Pose2d(new Translation2d(35 - dist, 0), new Rotation2d()); + sysUnderTest.processFrame(robotPose); + + var result = sysUnderTest.cam.getLatestResult(); + + assertTrue(result.hasTargets()); + assertEquals(result.getBestTarget().getCameraToTarget().getTranslation().getNorm(), dist); + } + + @Test + public void testVisibilityCupidShuffle() { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0)); + + // To the right, to the right + var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + + // To the right, to the right + robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + + // To the left, to the left + robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + + // To the left, to the left + robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + + // now kick, now kick + robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + // now kick, now kick + robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + // now walk it by yourself + robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + + // now walk it by yourself + sysUnderTest.moveCamera( + new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)), 0, 1.0); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + } + + @Test + public void testNotVisibleVert1() { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0)); + + var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + sysUnderTest.moveCamera(new Transform2d(), 5000, 1.0); // vooop selfie stick + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + } + + @Test + public void testNotVisibleVert2() { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 45.0, new Transform2d(), 1, 99999, 1234, 1234, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 0.5)); + + var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + // Pitched back camera should mean target goes out of view below the robot as distance increases + robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + } + + @Test + public void testNotVisibleTgtSize() { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 20.0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1)); + + var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + } + + @Test + public void testNotVisibleTooFarForLEDs() { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 10, 640, 480, 1.0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1)); + + var robotPose = new Pose2d(new Translation2d(28, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + + robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + } + + @ParameterizedTest + @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23}) + public void testYawAngles(double testYaw) { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4)); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5)); + + var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(testYaw)); + sysUnderTest.processFrame(robotPose); + var res = sysUnderTest.cam.getLatestResult(); + assertTrue(res.hasTargets()); + var tgt = res.getBestTarget(); + assertEquals(tgt.getYaw(), testYaw, 0.0001); + } + + @ParameterizedTest + @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) + public void testCameraPitch(double testPitch) { + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4)); + final var robotPose = new Pose2d(new Translation2d(30, 0), new Rotation2d(0)); + var sysUnderTest = + new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 0.0, 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5)); + + sysUnderTest.moveCamera(new Transform2d(), 0.0, testPitch); + sysUnderTest.processFrame(robotPose); + var res = sysUnderTest.cam.getLatestResult(); + assertTrue(res.hasTargets()); + var tgt = res.getBestTarget(); + // If the camera is pitched down by 10 degrees, the target should appear + // in the upper part of the image (ie, pitch positive). Therefor, + // pass/fail involves -1.0. + assertEquals(tgt.getPitch(), -1.0 * testPitch, 0.0001); + } + + private static Stream distCalCParamProvider() { + // Arbitrary and fairly random assortment of distances, camera pitches, and heights + return Stream.of( + Arguments.of(5, 35, 0), + Arguments.of(6, 35, 1), + Arguments.of(10, 35, 0), + Arguments.of(15, 35, 2), + Arguments.of(19.95, 35, 0), + Arguments.of(20, 35, 0), + Arguments.of(5, 42, 1), + Arguments.of(6, 42, 0), + Arguments.of(10, 42, 2), + Arguments.of(15, 42, 0.5), + Arguments.of(19.42, 35, 0), + Arguments.of(20, 42, 0), + Arguments.of(5, 55, 2), + Arguments.of(6, 55, 0), + Arguments.of(10, 54, 2.2), + Arguments.of(15, 53, 0), + Arguments.of(19.52, 35, 1.1), + Arguments.of(20, 51, 2.87), + Arguments.of(20, 55, 3)); + } + + @ParameterizedTest + @MethodSource("distCalCParamProvider") + public void testDistanceCalc(double testDist, double testPitch, double testHeight) { + // Assume dist along ground and tgt height the same. Iterate over other parameters. + + final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 42)); + final var robotPose = new Pose2d(new Translation2d(35 - testDist, 0), new Rotation2d(0)); + var sysUnderTest = + new SimVisionSystem( + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!", + 160.0, + testPitch, + new Transform2d(), + testHeight, + 99999, + 640, + 480, + 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, testDist, 0.5, 0.5)); + + sysUnderTest.processFrame(robotPose); + var res = sysUnderTest.cam.getLatestResult(); + assertTrue(res.hasTargets()); + var tgt = res.getBestTarget(); + assertEquals(tgt.getYaw(), 0.0, 0.0001); + double distMeas = + PhotonUtils.calculateDistanceToTargetMeters( + testHeight, + testDist, + Units.degreesToRadians(testPitch), + Units.degreesToRadians(tgt.getPitch())); + assertEquals(distMeas, testDist, 0.001); + } + + @Test + public void testMultipleTargets() { + final var targetPoseL = new Pose2d(new Translation2d(35, 2), new Rotation2d()); + final var targetPoseC = new Pose2d(new Translation2d(35, 0), new Rotation2d()); + final var targetPoseR = new Pose2d(new Translation2d(35, -2), new Rotation2d()); + var sysUnderTest = + new SimVisionSystem("Test", 160.0, 0.0, new Transform2d(), 5.0, 99999, 640, 480, 20.0); + + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 0.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 1.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 2.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 3.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 4.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 5.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 6.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 7.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 8.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 9.0, 0.25, 0.1)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 10.0, 0.25, 0.1)); + + var robotPose = new Pose2d(new Translation2d(30, 0), Rotation2d.fromDegrees(5)); + sysUnderTest.processFrame(robotPose); + var res = sysUnderTest.cam.getLatestResult(); + assertTrue(res.hasTargets()); + List tgtList; + tgtList = res.getTargets(); + assertEquals(tgtList.size(), 11); + } +} diff --git a/photon-lib/src/test/native/cpp/PacketTest.cpp b/photon-lib/src/test/native/cpp/PacketTest.cpp new file mode 100644 index 000000000..ba31e23de --- /dev/null +++ b/photon-lib/src/test/native/cpp/PacketTest.cpp @@ -0,0 +1,91 @@ +/* + * 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 . + */ + +#include + +#include + +#include "gtest/gtest.h" +#include "photonlib/PhotonPipelineResult.h" +#include "photonlib/PhotonTrackedTarget.h" + +TEST(PacketTest, PhotonTrackedTarget) { + photonlib::PhotonTrackedTarget target{ + 3.0, 4.0, 9.0, -5.0, + frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)}; + photonlib::Packet p; + p << target; + + photonlib::PhotonTrackedTarget b; + p >> b; + + for (auto& c : p.GetData()) { + std::cout << static_cast(c) << ","; + } + + EXPECT_EQ(target, b); +} + +TEST(PacketTest, PhotonPipelineResult) { + photonlib::PhotonPipelineResult result{1_s, {}}; + photonlib::Packet p; + p << result; + + photonlib::PhotonPipelineResult b; + p >> b; + + EXPECT_EQ(result, b); + + wpi::SmallVector targets{ + photonlib::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, + frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)}, + photonlib::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, + frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad)}}; + + photonlib::PhotonPipelineResult result2{2_s, targets}; + photonlib::Packet p2; + p2 << result2; + + photonlib::PhotonPipelineResult b2; + p2 >> b2; + + EXPECT_EQ(result2, b2); +} + +TEST(PacketTest, BytePackFromJava) { + std::vector bytePack{ + 64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0, + 0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0, + 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, + 0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47, 122}; + + std::vector bytes; + for (auto a : bytePack) bytes.emplace_back(static_cast(a)); + + photonlib::Packet packet{bytes}; + + photonlib::PhotonTrackedTarget res; + packet >> res; + + photonlib::PhotonTrackedTarget target{ + 3.0, 4.0, 9.0, -5.0, + frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)}; + + EXPECT_EQ(res, target); +} diff --git a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp new file mode 100644 index 000000000..db061a33d --- /dev/null +++ b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp @@ -0,0 +1,21 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photonlib/PhotonUtils.h" + +TEST(PhotonUtilsTest, TestInclude) {} diff --git a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp new file mode 100644 index 000000000..b6389aa38 --- /dev/null +++ b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp @@ -0,0 +1,401 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include +#include + +#include "gtest/gtest.h" +#include "photonlib/PhotonCamera.h" +#include "photonlib/PhotonUtils.h" +#include "photonlib/SimVisionSystem.h" + +TEST(SimVisionSystemTest, testEmpty) { + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 320, 240, 0.0); + + for (int loopIdx = 0; loopIdx < 100; loopIdx++) { + sysUnderTest.ProcessFrame(frc::Pose2d()); + } +} + +class SimVisionSystemTestDistParam : public testing::TestWithParam {}; +INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestDistParamInst, + SimVisionSystemTestDistParam, + testing::Values(5, 10, 15, 20, 25, 30)); + +TEST_P(SimVisionSystemTestDistParam, testDistanceAligned) { + double dist = GetParam(); + + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 320, 240, 0.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 0.0_m, 1.0_m, 1.0_m)); + + auto robotPose = frc::Pose2d( + frc::Translation2d(units::meter_t(35.0 - dist), 0_m), frc::Rotation2d()); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + ASSERT_EQ(result.GetBestTarget() + .GetCameraRelativePose() + .Translation() + .Norm() + .to(), + dist); +} + +TEST(SimVisionSystemTest, testVisibilityCupidShuffle) { + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 320, 240, 0.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 0.0_m, 3.0_m, 3.0_m)); + + // To the right, to the right + auto robotPose = + frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-70.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-95.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(90.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(65.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); + + // now kick, now kick + robotPose = + frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); + + // now kick, now kick + robotPose = + frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(-5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); + + // now walk it by yourself + robotPose = frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), + frc::Rotation2d(-179.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); + + // now walk it by yourself + sysUnderTest.MoveCamera( + frc::Transform2d(frc::Translation2d(), frc::Rotation2d(180_deg)), 0.0_m, + 1.0_deg); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); +} + +TEST(SimVisionSystemTest, testNotVisibleVert1) { + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 640, 480, 0.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 1.0_m, 3.0_m, 2.0_m)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + + sysUnderTest.MoveCamera( + frc::Transform2d(frc::Translation2d(), frc::Rotation2d(0_deg)), 5000.0_m, + 1.0_deg); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); +} + +TEST(SimVisionSystemTest, testNotVisibleVert2) { + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 45.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 1234, 1234, 0.5); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 3.0_m, 0.5_m, 0.5_m)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); + + robotPose = + frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); +} + +TEST(SimVisionSystemTest, testNotVisibleTgtSize) { + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 640, 480, 20.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); + + robotPose = + frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); +} + +TEST(SimVisionSystemTest, testNotVisibleTooFarForLEDs) { + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 10.0_m, + 640, 480, 1.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(28.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + EXPECT_TRUE(result.HasTargets()); + + robotPose = + frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg)); + sysUnderTest.ProcessFrame(robotPose); + result = sysUnderTest.cam.GetLatestResult(); + EXPECT_FALSE(result.HasTargets()); +} + +class SimVisionSystemTestYawParam : public testing::TestWithParam {}; +INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestYawParamInst, + SimVisionSystemTestYawParam, + testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23)); +TEST_P(SimVisionSystemTestYawParam, testYawAngles) { + double testYaw = GetParam(); // Nope, Chuck testYaw + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg)); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 640, 480, 0.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m)); + + auto robotPose = frc::Pose2d(frc::Translation2d(32_m, 0.0_m), + frc::Rotation2d(units::degree_t(testYaw))); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + auto tgt = result.GetBestTarget(); + EXPECT_DOUBLE_EQ(tgt.GetYaw(), testYaw); +} + +class SimVisionSystemTestCameraPitchParam + : public testing::TestWithParam {}; +INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestCameraPitchParamInst, + SimVisionSystemTestCameraPitchParam, + testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23, + 20.21, -19.999)); +TEST_P(SimVisionSystemTestCameraPitchParam, testCameraPitch) { + double testPitch = GetParam(); + auto targetPose = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(0.0_deg)); + + photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg, + frc::Transform2d(), 1.0_m, 99999.0_m, + 640, 480, 0.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m)); + + sysUnderTest.MoveCamera( + frc::Transform2d(frc::Translation2d(), frc::Rotation2d()), 0.0_m, + units::degree_t(testPitch)); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + auto tgt = result.GetBestTarget(); + // If the camera is pitched down by 10 degrees, the target should appear + // in the upper part of the image (ie, pitch positive). Therefor, + // pass/fail involves -1.0. + EXPECT_DOUBLE_EQ(tgt.GetPitch(), -1.0 * testPitch); +} + +class SimVisionSystemTestDistCalcParam + : public testing::TestWithParam> {}; +INSTANTIATE_TEST_SUITE_P( + SimVisionSystemTestDistCalcParamInst, SimVisionSystemTestDistCalcParam, + testing::Values(std::tuple(5, 35, 0), + std::tuple(6, 35, 1), + std::tuple(10, 35, 0), + std::tuple(15, 35, 2), + std::tuple(19.95, 35, 0), + std::tuple(20, 35, 0), + std::tuple(5, 42, 1), + std::tuple(6, 42, 0), + std::tuple(10, 42, 2), + std::tuple(15, 42, 0.5), + std::tuple(19.42, 35, 0), + std::tuple(20, 42, 0), + std::tuple(5, 55, 2), + std::tuple(6, 55, 0), + std::tuple(10, 54, 2.2), + std::tuple(15, 53, 0), + std::tuple(19.52, 35, 1.1), + std::tuple(20, 51, 2.87), + std::tuple(20, 55, 3))); +TEST_P(SimVisionSystemTestDistCalcParam, testDistanceCalc) { + std::tuple testArgs = GetParam(); + double testDist = std::get<0>(testArgs); + double testPitch = std::get<1>(testArgs); + double testHeight = std::get<2>(testArgs); + + auto targetPose = frc::Pose2d(frc::Translation2d(35_m, 0_m), + frc::Rotation2d(units::radian_t(3.14159 / 42))); + + auto robotPose = + frc::Pose2d(frc::Translation2d(units::meter_t(35 - testDist), 0.0_m), + frc::Rotation2d(0.0_deg)); + + photonlib::SimVisionSystem sysUnderTest( + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife", + 160.0_deg, units::degree_t(testPitch), frc::Transform2d(), + units::meter_t(testHeight), 99999.0_m, 640, 480, 0.0); + + sysUnderTest.AddSimVisionTarget(photonlib::SimVisionTarget( + targetPose, units::meter_t(testDist), 0.5_m, 0.5_m)); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + auto tgt = result.GetBestTarget(); + EXPECT_DOUBLE_EQ(tgt.GetYaw(), 0.0); + units::meter_t distMeas = photonlib::PhotonUtils::CalculateDistanceToTarget( + units::meter_t(testHeight), units::meter_t(testDist), + units::degree_t(testPitch), units::degree_t(tgt.GetPitch())); + EXPECT_DOUBLE_EQ(distMeas.to(), testDist); +} + +TEST(SimVisionSystemTest, testMultipleTargets) { + auto targetPoseL = + frc::Pose2d(frc::Translation2d(35_m, 2_m), frc::Rotation2d()); + auto targetPoseC = + frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); + auto targetPoseR = + frc::Pose2d(frc::Translation2d(35_m, -2_m), frc::Rotation2d()); + + photonlib::SimVisionSystem sysUnderTest("test", 160.0_deg, 0.0_deg, + frc::Transform2d(), 5.0_m, 99999.0_m, + 640, 480, 20.0); + + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseL, 0.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseC, 1.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseR, 2.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseL, 3.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseC, 4.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseR, 5.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseL, 6.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseC, 7.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseL, 8.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseR, 9.0_m, 0.25_m, 0.1_m)); + sysUnderTest.AddSimVisionTarget( + photonlib::SimVisionTarget(targetPoseL, 10.0_m, 0.25_m, 0.1_m)); + + auto robotPose = + frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(5.0_deg)); + + sysUnderTest.ProcessFrame(robotPose); + auto result = sysUnderTest.cam.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + + auto tgtList = result.GetTargets(); + EXPECT_EQ(11ul, tgtList.size()); +} diff --git a/photon-lib/src/test/native/cpp/main.cpp b/photon-lib/src/test/native/cpp/main.cpp new file mode 100644 index 000000000..b03cde4ea --- /dev/null +++ b/photon-lib/src/test/native/cpp/main.cpp @@ -0,0 +1,24 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/photon-targeting/.gitignore b/photon-targeting/.gitignore new file mode 100644 index 000000000..933a7e534 --- /dev/null +++ b/photon-targeting/.gitignore @@ -0,0 +1,13 @@ +bin/* +.settings/* +.project +.classpath +*.prefs +.gradle +.gradle/* +build +build/* +photonvision/* +photonvision_config/* + +src/main/java/org/photonvision/PhotonVersion.java \ No newline at end of file diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle new file mode 100644 index 000000000..f43133d4f --- /dev/null +++ b/photon-targeting/build.gradle @@ -0,0 +1,33 @@ +apply plugin: 'java' + +repositories { + maven { + url = 'https://frcmaven.wpi.edu:443/artifactory/development' + } + maven { + url = 'https://frcmaven.wpi.edu:443/artifactory/release' + } +} + +apply from: '../versioningHelper.gradle' + +ext { + pubVersion = versionString +} + +def openCVVersion = '3.4.7-2' + +dependencies { + implementation 'edu.wpi.first.wpimath:wpimath-java:2021.1.2' + implementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:$openCVVersion" + implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" + implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" + implementation 'org.apache.commons:commons-math3:3.6.1' +} + +java { + withJavadocJar() + withSourcesJar() +} + +apply from: 'publish.gradle' diff --git a/photon-targeting/publish.gradle b/photon-targeting/publish.gradle new file mode 100644 index 000000000..c54cf2988 --- /dev/null +++ b/photon-targeting/publish.gradle @@ -0,0 +1,28 @@ +apply plugin: 'maven-publish' + +def artifactGroupId = 'org.photonvision' +def baseArtifactId = 'PhotonTargeting' + +def outputsFolder = file("$buildDir/outputs") + +publishing { + repositories { + maven { + url 'https://maven.photonvision.org/repository/internal' + credentials { + username 'ghactions' + password System.getenv("ARTIFACTORY_API_KEY") + } + } + } + + publications { + mavenJava(MavenPublication) { + groupId = artifactGroupId + artifactId = "${baseArtifactId}-java" + version = pubVersion + + from components.java + } + } +} diff --git a/photon-targeting/settings.gradle b/photon-targeting/settings.gradle new file mode 100644 index 000000000..674b1ce85 --- /dev/null +++ b/photon-targeting/settings.gradle @@ -0,0 +1,2 @@ +rootProject.name = 'photon-targeting' + diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java similarity index 98% rename from photon-core/src/main/java/org/photonvision/common/dataflow/structures/Packet.java rename to photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index a09e9bd0c..07d9abc9f 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -49,6 +49,10 @@ public class Packet { writePos = 0; } + public int getSize() { + return size; + } + /** * Returns the packet data. * diff --git a/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java b/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java new file mode 100644 index 000000000..af95fc2cb --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java @@ -0,0 +1,46 @@ +/* + * 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.hardware; + +public enum VisionLEDMode { + kDefault(-1), + kOff(0), + kOn(1), + kBlink(2); + + public final int value; + + VisionLEDMode(int value) { + this.value = value; + } + + @Override + public String toString() { + switch (this) { + case kDefault: + return "Default"; + case kOff: + return "Off"; + case kOn: + return "On"; + case kBlink: + return "Blink"; + } + return ""; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java similarity index 59% rename from photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java rename to photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 4f52997d2..5764b7a24 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -15,55 +15,102 @@ * along with this program. If not, see . */ -package org.photonvision.vision.pipeline.result; +package org.photonvision.targeting; import java.util.ArrayList; import java.util.List; import java.util.Objects; import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.vision.target.TrackedTarget; -public class SimplePipelineResult { +/** Represents a pipeline result from a PhotonCamera. */ +public class PhotonPipelineResult { + private static boolean HAS_WARNED = false; + + // Targets to store. + public final List targets = new ArrayList<>(); + + // Latency in milliseconds. private double latencyMillis; + + // Whether targets exist. private boolean hasTargets; - public final List targets = new ArrayList<>(); - public SimplePipelineResult() {} + /** Constructs an empty pipeline result. */ + public PhotonPipelineResult() {} - public SimplePipelineResult( - double latencyMillis, boolean hasTargets, List targets) { + /** + * Constructs a pipeline result. + * + * @param latencyMillis The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + */ + public PhotonPipelineResult(double latencyMillis, List targets) { this.latencyMillis = latencyMillis; - this.hasTargets = hasTargets; + this.hasTargets = targets.size() != 0; this.targets.addAll(targets); } - public SimplePipelineResult(CVPipelineResult r) { - this(r.processingMillis, r.hasTargets(), simpleFromTrackedTargets(r.targets)); - } - /** * Returns the size of the packet needed to store this pipeline result. * * @return The size of the packet needed to store this pipeline result. */ public int getPacketSize() { - return targets.size() * SimpleTrackedTarget.PACK_SIZE_BYTES + 8 + 2; + return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2; } + /** + * Returns the best target in this pipeline result. If there are no targets, this method will + * return null. The best target is determined by the target sort mode in the PhotonVision UI. + * + * @return The best target of the pipeline result. + */ + public PhotonTrackedTarget getBestTarget() { + if (!hasTargets && !HAS_WARNED) { + 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"; + System.err.println(errStr); + new Exception().printStackTrace(); + HAS_WARNED = true; + } + return hasTargets ? targets.get(0) : null; + } + + /** + * Returns the latency in the pipeline. + * + * @return The latency in the pipeline. + */ public double getLatencyMillis() { return latencyMillis; } + /** + * Returns whether the pipeline has targets. + * + * @return Whether the pipeline has targets. + */ public boolean hasTargets() { return hasTargets; } + /** + * Returns a copy of the vector of targets. + * + * @return A copy of the vector of targets. + */ + public List getTargets() { + return new ArrayList<>(targets); + } + @Override public boolean equals(Object o) { if (this == o) return true; if (o == null || getClass() != o.getClass()) return false; - SimplePipelineResult that = (SimplePipelineResult) o; + PhotonPipelineResult that = (PhotonPipelineResult) o; boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0; boolean hasTargetsMatch = that.hasTargets == hasTargets; boolean targetsMatch = that.targets.equals(targets); @@ -91,7 +138,7 @@ public class SimplePipelineResult { // Decode the information of each target. for (int i = 0; i < (int) targetCount; ++i) { - var target = new SimpleTrackedTarget(); + var target = new PhotonTrackedTarget(); target.createFromPacket(packet); targets.add(target); } @@ -117,12 +164,4 @@ public class SimplePipelineResult { // Return the packet. return packet; } - - private static List simpleFromTrackedTargets(List targets) { - var ret = new ArrayList(); - for (var t : targets) { - ret.add(new SimpleTrackedTarget(t)); - } - return ret; - } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java similarity index 88% rename from photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java rename to photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 1f0ba9665..3baf4f2b3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -15,16 +15,15 @@ * along with this program. If not, see . */ -package org.photonvision.vision.pipeline.result; +package org.photonvision.targeting; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import java.util.Objects; import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.vision.target.TrackedTarget; -public class SimpleTrackedTarget { +public class PhotonTrackedTarget { public static final int PACK_SIZE_BYTES = Double.BYTES * 7; private double yaw; @@ -33,9 +32,9 @@ public class SimpleTrackedTarget { private double skew; private Transform2d cameraToTarget = new Transform2d(); - public SimpleTrackedTarget() {} + public PhotonTrackedTarget() {} - public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) { + public PhotonTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) { this.yaw = yaw; this.pitch = pitch; this.area = area; @@ -43,10 +42,6 @@ public class SimpleTrackedTarget { cameraToTarget = pose; } - public SimpleTrackedTarget(TrackedTarget t) { - this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget()); - } - public double getYaw() { return yaw; } @@ -67,7 +62,7 @@ public class SimpleTrackedTarget { public boolean equals(Object o) { if (this == o) return true; if (o == null || getClass() != o.getClass()) return false; - SimpleTrackedTarget that = (SimpleTrackedTarget) o; + PhotonTrackedTarget that = (PhotonTrackedTarget) o; return Double.compare(that.yaw, yaw) == 0 && Double.compare(that.pitch, pitch) == 0 && Double.compare(that.area, area) == 0 diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle new file mode 100644 index 000000000..379e268a3 --- /dev/null +++ b/photonlib-cpp-examples/build.gradle @@ -0,0 +1,77 @@ +plugins { + id 'cpp' + id 'java' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2' + id 'google-test-test-suite' + id 'edu.wpi.first.NativeUtils' version '2020.10.1' + id 'edu.wpi.first.GradleJni' version '0.10.1' + id 'edu.wpi.first.GradleVsCode' version '0.12.0' +} + +repositories { + maven { + url = 'https://frcmaven.wpi.edu:443/artifactory/release' + maven { url "https://frcmaven.wpi.edu/artifactory/development" } + mavenCentral() + } +} + +apply from: '../photon-lib/config.gradle' + +ext.examplesMap = [:] + +File examplesTree = file("$projectDir/src/main/cpp/examples") +examplesTree.list(new FilenameFilter() { + @Override + public boolean accept(File current, String name) { + return new File(current, name).isDirectory(); + } +}).each { + examplesMap.put(it, []) +} + +nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.roborio).configure { + cppCompiler.args.remove('-Wno-error=deprecated-declarations') + cppCompiler.args.add('-Werror=deprecated-declarations') +} + +ext { + sharedCvConfigs = examplesMap + staticCvConfigs = [:] + useJava = false + useCpp = true +} + +model { + components { + examplesMap.each { key, value -> + "${key}"(NativeExecutableSpec) { + targetBuildTypes 'debug' + binaries.all { binary -> + lib project: ':photon-lib', library: 'Photon', linkage: 'shared' + if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared') + } + } + sources { + cpp { + source { + srcDirs 'src/main/cpp/examples/' + "${key}" + "/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/cpp/examples/' + "${key}" + "/include" + include '**/*.h' + } + } + } + nativeUtils.useRequiredLibrary(it, 'wpilib_shared') + } + } + } +} + +ext { + exampleDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/") + exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json") +} \ No newline at end of file diff --git a/photonlib-cpp-examples/settings.gradle b/photonlib-cpp-examples/settings.gradle new file mode 100644 index 000000000..325404797 --- /dev/null +++ b/photonlib-cpp-examples/settings.gradle @@ -0,0 +1 @@ +rootproject.name = 'photonlib-cpp-examples' \ No newline at end of file diff --git a/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp b/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp new file mode 100644 index 000000000..a9b077233 --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp @@ -0,0 +1,64 @@ +/** + * Copyright (C) 2018-2020 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 . + */ + +#include "Robot.h" + +#include + +void Robot::TeleopPeriodic() { + double forwardSpeed; + double rotationSpeed; + + if (xboxController.GetAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + const auto &result = camera.GetLatestResult(); + + if (result.HasTargets()) { + // First calculate range + units::meter_t range = + photonlib::PhotonUtils::CalculateDistanceToTarget( + CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, + units::degree_t{result.GetBestTarget().GetPitch()}); + + // Use this range as the measurement we give to the PID controller. + forwardSpeed = forwardController.Calculate( + range.to(), GOAL_RANGE_METERS.to()); + + // Also calculate angular power + rotationSpeed = + turnController.Calculate(result.GetBestTarget().GetYaw(), 0); + } else { + // If we have no targets, stay still. + forwardSpeed = 0; + rotationSpeed = 0; + } + } else { + // Manual Driver Mode + forwardSpeed = + xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand); + rotationSpeed = + xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand); + } + + // Use our forward/turn speeds to control the drivetrain + drive.ArcadeDrive(forwardSpeed, rotationSpeed); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/include/Robot.h b/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/include/Robot.h new file mode 100644 index 000000000..9a42f4fcf --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/aimandrange/include/Robot.h @@ -0,0 +1,64 @@ +/** + * Copyright (C) 2018-2020 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 +#include + +class Robot : public frc::TimedRobot { +public: + void TeleopPeriodic() override; + +private: + // Constants such as camera and target height stored. Change per robot and + // goal! + const units::meter_t CAMERA_HEIGHT = 24_in; + const units::meter_t TARGET_HEIGHT = 5_ft; + + // Angle between horizontal and the camera. + const units::radian_t CAMERA_PITCH = 0_deg; + + // How far from the target we want to be + const units::meter_t GOAL_RANGE_METERS = 3_ft; + + // PID constants should be tuned per robot + const double LINEAR_P = 0.1; + const double LINEAR_D = 0.0; + frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D}; + + const double ANGULAR_P = 0.1; + const double ANGULAR_D = 0.0; + frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; + + // Change this to match the name of your camera + photonlib::PhotonCamera camera{"photonvision"}; + + frc::XboxController xboxController{0}; + + // Drive motors + frc::PWMVictorSPX leftMotor{0}; + frc::PWMVictorSPX rightMotor{1}; + frc::DifferentialDrive drive{leftMotor, rightMotor}; +}; diff --git a/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp b/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp new file mode 100644 index 000000000..6b068dec4 --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp @@ -0,0 +1,51 @@ +/** + * Copyright (C) 2018-2020 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 . + */ + +#include "Robot.h" + +#include + +void Robot::TeleopPeriodic() { + double forwardSpeed = + xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand); + double rotationSpeed; + + if (xboxController.GetAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + photonlib::PhotonPipelineResult result = camera.GetLatestResult(); + + if (result.HasTargets()) { + // Rotation speed is the output of the PID controller + rotationSpeed = controller.Calculate(result.GetBestTarget().GetYaw(), 0); + } else { + // If we have no targets, stay still. + rotationSpeed = 0; + } + } else { + // Manual Driver Mode + rotationSpeed = + xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand); + } + + // Use our forward/turn speeds to control the drivetrain + drive.ArcadeDrive(forwardSpeed, rotationSpeed); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/include/Robot.h b/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/include/Robot.h new file mode 100644 index 000000000..b0cac12a4 --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/aimattarget/include/Robot.h @@ -0,0 +1,46 @@ +/** + * Copyright (C) 2018-2020 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 +#include + +class Robot : public frc::TimedRobot { +public: + void TeleopPeriodic() override; + +private: + // Change this to match the name of your camera + photonlib::PhotonCamera camera{"photonvision"}; + // PID constants should be tuned per robot + frc2::PIDController controller{.1, 0, 0}; + + frc::XboxController xboxController{0}; + + // Drive motors + frc::PWMVictorSPX leftMotor{0}; + frc::PWMVictorSPX rightMotor{1}; + frc::DifferentialDrive drive{leftMotor, rightMotor}; +}; diff --git a/photonlib-cpp-examples/src/main/cpp/examples/examples.json b/photonlib-cpp-examples/src/main/cpp/examples/examples.json new file mode 100644 index 000000000..2460afe15 --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/examples.json @@ -0,0 +1,38 @@ +[ + { + "name": "AimAtTarget", + "description": "Aim at a target", + "tags": [], + "gradlebase": "cpp", + "language": "cpp", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "aimattarget" + }, + { + "name": "GetInRange", + "description": "Get in range of a target", + "tags": [], + "gradlebase": "cpp", + "language": "cpp", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "getinrange" + }, + { + "name": "AimAndRange", + "description": "Aim at a target while at a desired range", + "tags": [], + "gradlebase": "cpp", + "language": "cpp", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "aimandrange" + } +] diff --git a/photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp b/photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp new file mode 100644 index 000000000..e0f7c6caa --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp @@ -0,0 +1,57 @@ +/** + * Copyright (C) 2018-2020 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 . + */ + +#include "Robot.h" + +#include + +void Robot::TeleopPeriodic() { + double forwardSpeed; + double rotationSpeed = + xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand); + + if (xboxController.GetAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + photonlib::PhotonPipelineResult result = camera.GetLatestResult(); + + if (result.HasTargets()) { + // First calculate range + units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget( + CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, + units::degree_t{result.GetBestTarget().GetPitch()}); + + // Use this range as the measurement we give to the PID controller. + forwardSpeed = controller.Calculate(range.to(), + GOAL_RANGE_METERS.to()); + } else { + // If we have no targets, stay still. + forwardSpeed = 0; + } + } else { + // Manual Driver Mode + forwardSpeed = + xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand); + } + + // Use our forward/turn speeds to control the drivetrain + drive.ArcadeDrive(forwardSpeed, rotationSpeed); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/src/main/cpp/examples/getinrange/include/Robot.h b/photonlib-cpp-examples/src/main/cpp/examples/getinrange/include/Robot.h new file mode 100644 index 000000000..10272a4af --- /dev/null +++ b/photonlib-cpp-examples/src/main/cpp/examples/getinrange/include/Robot.h @@ -0,0 +1,60 @@ +/** + * Copyright (C) 2018-2020 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 +#include + +class Robot : public frc::TimedRobot { +public: + void TeleopPeriodic() override; + +private: + // Constants such as camera and target height stored. Change per robot and + // goal! + const units::meter_t CAMERA_HEIGHT = 24_in; + const units::meter_t TARGET_HEIGHT = 5_ft; + + // Angle between horizontal and the camera. + const units::radian_t CAMERA_PITCH = 0_deg; + + // How far from the target we want to be + const units::meter_t GOAL_RANGE_METERS = 3_ft; + + // PID constants should be tuned per robot + const double P_GAIN = 0.1; + const double D_GAIN = 0.0; + frc2::PIDController controller{P_GAIN, 0.0, D_GAIN}; + + // Change this to match the name of your camera + photonlib::PhotonCamera camera{"photonvision"}; + + frc::XboxController xboxController{0}; + + // Drive motors + frc::PWMVictorSPX leftMotor{0}; + frc::PWMVictorSPX rightMotor{1}; + frc::DifferentialDrive drive{leftMotor, rightMotor}; +}; diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle new file mode 100644 index 000000000..75473cf58 --- /dev/null +++ b/photonlib-java-examples/build.gradle @@ -0,0 +1,35 @@ +apply plugin: 'java' + +String wpilibVersion = "2021.1.2" +String opencvVersion = "3.4.7-5" +String ejmlVersion = "0.38" +String jacksonVersion = "2.10.0" + +repositories { + maven { + url = 'https://frcmaven.wpi.edu:443/artifactory/release' + } +} + +dependencies { + implementation project(':photon-lib') + implementation project(':photon-targeting') + + implementation "edu.wpi.first.wpilibj:wpilibj-java:${wpilibVersion}" + implementation "edu.wpi.first.wpimath:wpimath-java:${wpilibVersion}" + implementation "edu.wpi.first.ntcore:ntcore-java:${wpilibVersion}" + implementation "edu.wpi.first.wpiutil:wpiutil-java:${wpilibVersion}" + implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}" + implementation "edu.wpi.first.cscore:cscore-java:${wpilibVersion}" + implementation "edu.wpi.first.cameraserver:cameraserver-java:${wpilibVersion}" + implementation "edu.wpi.first.hal:hal-java:${wpilibVersion}" + implementation "org.ejml:ejml-simple:${ejmlVersion}" + implementation "com.fasterxml.jackson.core:jackson-annotations:${jacksonVersion}" + implementation "com.fasterxml.jackson.core:jackson-core:${jacksonVersion}" + implementation "com.fasterxml.jackson.core:jackson-databind:${jacksonVersion}" +} + +ext { + exampleDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/") + exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json") +} \ No newline at end of file diff --git a/photonlib-java-examples/settings.gradle b/photonlib-java-examples/settings.gradle new file mode 100644 index 000000000..fe6bf76ca --- /dev/null +++ b/photonlib-java-examples/settings.gradle @@ -0,0 +1 @@ +rootproject.name = 'photonlib-java-examples' \ No newline at end of file diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Main.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Main.java new file mode 100644 index 000000000..b11b5b18a --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Main.java @@ -0,0 +1,38 @@ +/* + * 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.photonlib.examples.aimandrange; + +import edu.wpi.first.wpilibj.RobotBase; + +/** +* Do NOT add any static variables to this class, or any initialization at all. Unless you know what +* you are doing, do not modify this file except to change the parameter class to the startRobot +* call. +*/ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java new file mode 100644 index 000000000..0e40a393e --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java @@ -0,0 +1,103 @@ +/* + * 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.photonlib.examples.aimandrange; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.util.Units; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; + +/** +* The VM is configured to automatically run this class, and to call the functions corresponding to +* each mode, as described in the TimedRobot documentation. If you change the name of this class or +* the package after creating this project, you must also update the build.gradle file in the +* project. +*/ +public class Robot extends TimedRobot { + // Constants such as camera and target height stored. Change per robot and goal! + final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); + final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); + // Angle between horizontal and the camera. + final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + + // How far from the target we want to be + final double GOAL_RANGE_METERS = Units.feetToMeters(3); + + // Change this to match the name of your camera + PhotonCamera camera = new PhotonCamera("photonvision"); + + // PID constants should be tuned per robot + final double LINEAR_P = 0.1; + final double LINEAR_D = 0.0; + PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); + + final double ANGULAR_P = 0.1; + final double ANGULAR_D = 0.0; + PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + + XboxController xboxController = new XboxController(0); + + // Drive motors + PWMVictorSPX leftMotor = new PWMVictorSPX(0); + PWMVictorSPX rightMotor = new PWMVictorSPX(1); + DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + + @Override + public void teleopPeriodic() { + double forwardSpeed; + double rotationSpeed; + + if (xboxController.getAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + var result = camera.getLatestResult(); + + if (result.hasTargets()) { + // First calculate range + double range = + PhotonUtils.calculateDistanceToTargetMeters( + CAMERA_HEIGHT_METERS, + TARGET_HEIGHT_METERS, + CAMERA_PITCH_RADIANS, + result.getBestTarget().getPitch()); + + // Use this range as the measurement we give to the PID controller. + forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS); + + // Also calculate angular power + rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0); + } else { + // If we have no targets, stay still. + forwardSpeed = 0; + rotationSpeed = 0; + } + } else { + // Manual Driver Mode + forwardSpeed = xboxController.getY(GenericHID.Hand.kRight); + rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft); + } + + // Use our forward/turn speeds to control the drivetrain + drive.arcadeDrive(forwardSpeed, rotationSpeed); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Main.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Main.java new file mode 100644 index 000000000..bdb793801 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Main.java @@ -0,0 +1,38 @@ +/* + * 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.photonlib.examples.aimattarget; + +import edu.wpi.first.wpilibj.RobotBase; + +/** +* Do NOT add any static variables to this class, or any initialization at all. Unless you know what +* you are doing, do not modify this file except to change the parameter class to the startRobot +* call. +*/ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java new file mode 100644 index 000000000..50ad44b26 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java @@ -0,0 +1,103 @@ +/* + * 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.photonlib.examples.aimattarget; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.util.Units; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; + +/** +* The VM is configured to automatically run this class, and to call the functions corresponding to +* each mode, as described in the TimedRobot documentation. If you change the name of this class or +* the package after creating this project, you must also update the build.gradle file in the +* project. +*/ +public class Robot extends TimedRobot { + // Constants such as camera and target height stored. Change per robot and goal! + final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); + final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); + // Angle between horizontal and the camera. + final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + + // How far from the target we want to be + final double GOAL_RANGE_METERS = Units.feetToMeters(3); + + // Change this to match the name of your camera + PhotonCamera camera = new PhotonCamera("photonvision"); + + // PID constants should be tuned per robot + final double LINEAR_P = 0.1; + final double LINEAR_D = 0.0; + PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); + + final double ANGULAR_P = 0.1; + final double ANGULAR_D = 0.0; + PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + + XboxController xboxController = new XboxController(0); + + // Drive motors + PWMVictorSPX leftMotor = new PWMVictorSPX(0); + PWMVictorSPX rightMotor = new PWMVictorSPX(1); + DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + + @Override + public void teleopPeriodic() { + double forwardSpeed; + double rotationSpeed; + + if (xboxController.getAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + var result = camera.getLatestResult(); + + if (result.hasTargets()) { + // First calculate range + double range = + PhotonUtils.calculateDistanceToTargetMeters( + CAMERA_HEIGHT_METERS, + TARGET_HEIGHT_METERS, + CAMERA_PITCH_RADIANS, + result.getBestTarget().getPitch()); + + // Use this range as the measurement we give to the PID controller. + forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS); + + // Also calculate angular power + rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0); + } else { + // If we have no targets, stay still. + forwardSpeed = 0; + rotationSpeed = 0; + } + } else { + // Manual Driver Mode + forwardSpeed = xboxController.getY(GenericHID.Hand.kRight); + rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft); + } + + // Use our forward/turn speeds to control the drivetrain + drive.arcadeDrive(forwardSpeed, rotationSpeed); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json new file mode 100644 index 000000000..21570e837 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json @@ -0,0 +1,38 @@ +[ + { + "name": "AimAtTarget", + "description": "Aim at a target", + "tags": [], + "gradlebase": "java", + "language": "java", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "aimattarget" + }, + { + "name": "GetInRange", + "description": "Get in range of a target", + "tags": [], + "gradlebase": "java", + "language": "java", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "getinrange" + }, + { + "name": "AimAndRange", + "description": "Aim at a target while at a desired range", + "tags": [], + "gradlebase": "java", + "language": "java", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "aimandrange" + } +] \ No newline at end of file diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Main.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Main.java new file mode 100644 index 000000000..12e41d08e --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Main.java @@ -0,0 +1,38 @@ +/* + * 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.photonlib.examples.getinrange; + +import edu.wpi.first.wpilibj.RobotBase; + +/** +* Do NOT add any static variables to this class, or any initialization at all. Unless you know what +* you are doing, do not modify this file except to change the parameter class to the startRobot +* call. +*/ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java new file mode 100644 index 000000000..ba7785925 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java @@ -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 . + */ + +package org.photonlib.examples.getinrange; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.util.Units; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; + +/** +* The VM is configured to automatically run this class, and to call the functions corresponding to +* each mode, as described in the TimedRobot documentation. If you change the name of this class or +* the package after creating this project, you must also update the build.gradle file in the +* project. +*/ +public class Robot extends TimedRobot { + // Constants such as camera and target height stored. Change per robot and goal! + final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); + final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); + + // Angle between horizontal and the camera. + final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + + // How far from the target we want to be + final double GOAL_RANGE_METERS = Units.feetToMeters(3); + + // Change this to match the name of your camera + PhotonCamera camera = new PhotonCamera("photonvision"); + + // PID constants should be tuned per robot + final double P_GAIN = 0.1; + final double D_GAIN = 0.0; + PIDController controller = new PIDController(P_GAIN, 0, D_GAIN); + + XboxController xboxController; + + // Drive motors + PWMVictorSPX leftMotor = new PWMVictorSPX(0); + PWMVictorSPX rightMotor = new PWMVictorSPX(1); + DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + + @Override + public void robotInit() { + xboxController = new XboxController(0); + } + + @Override + public void teleopPeriodic() { + double forwardSpeed; + double rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft); + + if (xboxController.getAButton()) { + // Vision-alignment mode + // Query the latest result from PhotonVision + var result = camera.getLatestResult(); + + if (result.hasTargets()) { + // First calculate range + double range = + PhotonUtils.calculateDistanceToTargetMeters( + CAMERA_HEIGHT_METERS, + TARGET_HEIGHT_METERS, + CAMERA_PITCH_RADIANS, + result.getBestTarget().getPitch()); + + // Use this range as the measurement we give to the PID controller. + forwardSpeed = controller.calculate(range, GOAL_RANGE_METERS); + } else { + // If we have no targets, stay still. + forwardSpeed = 0; + } + } else { + // Manual Driver Mode + forwardSpeed = xboxController.getY(GenericHID.Hand.kRight); + } + + // Use our forward/turn speeds to control the drivetrain + drive.arcadeDrive(forwardSpeed, rotationSpeed); + } +} diff --git a/settings.gradle b/settings.gradle index 1ccfe01f5..4e061e958 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,2 +1,6 @@ +include 'photon-targeting' include 'photon-core' -include 'photon-server' \ No newline at end of file +include 'photon-server' +include 'photon-lib' +include 'photonlib-java-examples' +include 'photonlib-cpp-examples' diff --git a/versioningHelper.gradle b/versioningHelper.gradle new file mode 100644 index 000000000..d70670f73 --- /dev/null +++ b/versioningHelper.gradle @@ -0,0 +1,27 @@ +import java.time.LocalDateTime +import java.time.format.DateTimeFormatter +import java.nio.file.Path + +gradle.allprojects { + ext.getCurrentVersion = { -> + def stdout = new ByteArrayOutputStream() + String tagIsh + try { + exec { + commandLine 'git', 'describe', '--tags', '--exclude="Dev"' + standardOutput = stdout + } + tagIsh = stdout.toString().trim().toLowerCase() + } catch(Exception e) { + tagIsh = "dev-Unknown" + } + boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*") + if(isDev) tagIsh = "dev-" + tagIsh + println("Picked up version: " + tagIsh) + return tagIsh + } + + if(!ext.has("versionString")) { + ext.versionString = getCurrentVersion() + } +}