mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-25 01:41:40 +00:00
Compare commits
11 Commits
v2025.0.0-
...
v2025.0.0-
| Author | SHA1 | Date | |
|---|---|---|---|
|
5762167186 | ||
|
|
faa9eb0093 | ||
|
|
005363c5cd | ||
|
|
478723ca2c | ||
|
|
05dcfa2a13 | ||
|
|
eff95c09f1 | ||
|
|
097e641789 | ||
|
|
f107c94d05 | ||
|
|
93242edc86 | ||
|
|
eb395414ab | ||
|
|
04191efc51 |
40
.github/workflows/build.yml
vendored
40
.github/workflows/build.yml
vendored
@@ -68,23 +68,14 @@ jobs:
|
||||
- name: Install RoboRIO Toolchain
|
||||
run: ./gradlew installRoboRioToolchain
|
||||
# Need to publish to maven local first, so that C++ sim can pick it up
|
||||
# Still haven't figured out how to make the vendordep file be copied before trying to build examples
|
||||
- name: Publish photonlib to maven local
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:publishtomavenlocal photon-lib:publishtomavenlocal -x check
|
||||
run: ./gradlew photon-targeting:publishtomavenlocal photon-lib:publishtomavenlocal -x check
|
||||
- name: Build Java examples
|
||||
working-directory: photonlib-java-examples
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew copyPhotonlib -x check
|
||||
./gradlew build
|
||||
run: ./gradlew build
|
||||
- name: Build C++ examples
|
||||
working-directory: photonlib-cpp-examples
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew copyPhotonlib -x check
|
||||
./gradlew build
|
||||
run: ./gradlew build
|
||||
build-gradle:
|
||||
name: "Gradle Build"
|
||||
runs-on: ubuntu-22.04
|
||||
@@ -104,9 +95,7 @@ jobs:
|
||||
- name: Install mrcal deps
|
||||
run: sudo apt-get update && sudo apt-get install -y libcholmod3 liblapack3 libsuitesparseconfig5
|
||||
- name: Gradle Build
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:build photon-core:build photon-server:build -x check
|
||||
run: ./gradlew photon-targeting:build photon-core:build photon-server:build -x check
|
||||
- name: Gradle Tests
|
||||
run: ./gradlew testHeadless -i --stacktrace
|
||||
- name: Gradle Coverage
|
||||
@@ -165,7 +154,6 @@ jobs:
|
||||
|
||||
# Generate the JSON and give it the ""standard""" name maven gives it
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-lib:generateVendorJson
|
||||
export VERSION=$(git describe --tags --match=v*)
|
||||
mv photon-lib/build/generated/vendordeps/photonlib.json photon-lib/build/generated/vendordeps/photonlib-$(git describe --tags --match=v*).json
|
||||
@@ -205,9 +193,7 @@ jobs:
|
||||
distribution: temurin
|
||||
architecture: ${{ matrix.architecture }}
|
||||
- run: git fetch --tags --force
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:build photon-lib:build -i
|
||||
- run: ./gradlew photon-targeting:build photon-lib:build -i
|
||||
name: Build with Gradle
|
||||
- run: ./gradlew photon-lib:publish photon-targeting:publish
|
||||
name: Publish
|
||||
@@ -248,13 +234,9 @@ jobs:
|
||||
git config --global --add safe.directory /__w/photonvision/photonvision
|
||||
- name: Build PhotonLib
|
||||
# We don't need to run tests, since we specify only non-native platforms
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:build photon-lib:build ${{ matrix.build-options }} -i -x test
|
||||
run: ./gradlew photon-targeting:build photon-lib:build ${{ matrix.build-options }} -i -x test
|
||||
- name: Publish
|
||||
run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-lib:publish photon-targeting:publish ${{ matrix.build-options }}
|
||||
run: ./gradlew photon-lib:publish photon-targeting:publish ${{ matrix.build-options }}
|
||||
env:
|
||||
ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }}
|
||||
if: github.event_name == 'push' && github.repository_owner == 'photonvision'
|
||||
@@ -350,13 +332,9 @@ jobs:
|
||||
with:
|
||||
name: built-docs
|
||||
path: photon-server/src/main/resources/web/docs
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:jar photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
|
||||
- run: ./gradlew photon-targeting:jar photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
|
||||
if: ${{ (matrix.arch-override != 'none') }}
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-server:shadowJar
|
||||
- run: ./gradlew photon-server:shadowJar
|
||||
if: ${{ (matrix.arch-override == 'none') }}
|
||||
- uses: actions/upload-artifact@v4
|
||||
with:
|
||||
|
||||
4
.github/workflows/lint-format.yml
vendored
4
.github/workflows/lint-format.yml
vendored
@@ -61,9 +61,7 @@ jobs:
|
||||
with:
|
||||
java-version: 17
|
||||
distribution: temurin
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew spotlessCheck
|
||||
- run: ./gradlew spotlessCheck
|
||||
name: Run spotless
|
||||
|
||||
client-lint-format:
|
||||
|
||||
3
.github/workflows/photonvision-docs.yml
vendored
3
.github/workflows/photonvision-docs.yml
vendored
@@ -12,6 +12,9 @@ on:
|
||||
- 'docs/**'
|
||||
- '.github/**'
|
||||
|
||||
env:
|
||||
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
|
||||
|
||||
jobs:
|
||||
build:
|
||||
runs-on: ubuntu-22.04
|
||||
|
||||
24
.gitignore
vendored
24
.gitignore
vendored
@@ -131,27 +131,12 @@ New client/photon-client/*
|
||||
*.jfr
|
||||
.DS_Store
|
||||
# *.iml
|
||||
photon-server/build
|
||||
photon-server/photon-vision
|
||||
photon-server/src/main/resources/web
|
||||
photon-server/src/main/java/org/photonvision/PhotonVersion.java
|
||||
photon-server/src/main/generated/native/include/org_photonvision_raspi_PicamJNI.h
|
||||
*.bin
|
||||
.gradle
|
||||
.gradle/*
|
||||
photonvision_config
|
||||
build/spotlessJava
|
||||
build/*
|
||||
build
|
||||
photon-lib/src/main/java/org/photonvision/PhotonVersion.java
|
||||
photon-lib/bin/main/images/*
|
||||
/photonlib-java-examples/bin/
|
||||
photon-lib/src/generate/native/include/PhotonVersion.h
|
||||
.gitattributes
|
||||
lib/*
|
||||
photon-server/lib/libapriltag.so
|
||||
photon-server/bin/main/nativelibraries/apriltag/*
|
||||
photon-server/src/main/resources/nativelibraries/apriltag/*
|
||||
bin*/
|
||||
build*/
|
||||
|
||||
photonlib-java-examples/*/vendordeps/*
|
||||
photonlib-cpp-examples/*/vendordeps/*
|
||||
@@ -161,10 +146,7 @@ photonlib-cpp-examples/*/vendordeps/*
|
||||
photonlib-cpp-examples/*/networktables.json.bck
|
||||
photonlib-java-examples/*/networktables.json.bck
|
||||
*.sqlite
|
||||
photon-server/src/main/resources/web/index.html
|
||||
photon-lib/src/generate/native/cpp/PhotonVersion.cpp
|
||||
|
||||
photon-server/src/main/resources/web/*
|
||||
venv
|
||||
|
||||
.venv/*
|
||||
.venv
|
||||
|
||||
@@ -39,7 +39,7 @@ ext {
|
||||
libcameraDriverVersion = "dev-v2023.1.0-15-gc8988b3"
|
||||
rknnVersion = "dev-v2024.0.1-4-g0db16ac"
|
||||
frcYear = "2025"
|
||||
mrcalVersion = "dev-v2024.0.0-24-gc1efcf0";
|
||||
mrcalVersion = "dev-v2024.0.0-27-g41d7868";
|
||||
|
||||
|
||||
pubVersion = versionString
|
||||
@@ -67,7 +67,7 @@ spotless {
|
||||
java {
|
||||
target fileTree('.') {
|
||||
include '**/*.java'
|
||||
exclude '**/build/**', '**/build-*/**', "photon-core\\src\\main\\java\\org\\photonvision\\PhotonVersion.java", "photon-lib\\src\\main\\java\\org\\photonvision\\PhotonVersion.java", "**/src/generated/**"
|
||||
exclude '**/build/**', '**/build-*/**', '**/src/generated/**'
|
||||
}
|
||||
toggleOffOn()
|
||||
googleJavaFormat()
|
||||
|
||||
@@ -10,7 +10,8 @@
|
||||
# add these directories to sys.path here. If the directory is relative to the
|
||||
# documentation root, use os.path.abspath to make it absolute, like shown here.
|
||||
#
|
||||
# import os
|
||||
import os
|
||||
|
||||
# import sys
|
||||
# sys.path.insert(0, os.path.abspath('.'))
|
||||
|
||||
@@ -138,9 +139,15 @@ suppress_warnings = ["epub.unknown_project_files"]
|
||||
|
||||
sphinx_tabs_valid_builders = ["epub", "linkcheck"]
|
||||
|
||||
# -- Options for linkcheck -------------------------------------------------
|
||||
|
||||
# Excluded links for linkcheck
|
||||
# These should be periodically checked by hand to ensure that they are still functional
|
||||
linkcheck_ignore = ["https://www.raspberrypi.com/software/"]
|
||||
linkcheck_ignore = [R"https://www.raspberrypi.com/software/", R"http://10\..+"]
|
||||
|
||||
token = os.environ.get("GITHUB_TOKEN", None)
|
||||
if token:
|
||||
linkcheck_auth = [(R"https://github.com/.+", token)]
|
||||
|
||||
# MyST configuration (https://myst-parser.readthedocs.io/en/latest/configuration.html)
|
||||
myst_enable_extensions = ["colon_fence"]
|
||||
|
||||
@@ -8,16 +8,36 @@ The WPILibPi image includes FRCVision, which reserves USB cameras; to use Photon
|
||||
|
||||
SSH into the Raspberry Pi (using Windows command line, or a tool like [Putty](https://www.chiark.greenend.org.uk/~sgtatham/putty/) ) at the Romi's default address `10.0.0.2`. The default user is `pi`, and the password is `raspberry`.
|
||||
|
||||
Follow the process for installing PhotonVision on {ref}`"Other Debian-Based Co-Processor Installation" <docs/advanced-installation/sw_install/other-coprocessors:Other Debian-Based Co-Processor Installation>`. As it mentions this will require an internet connection so plugging into the ethernet jack on the Raspberry Pi will be the easiest solution. The pi must remain writable!
|
||||
:::.. The following paragraph can be restored when WPILibPi becomes compatible with the current version of PhotonVision.
|
||||
:::.. Follow the process for installing PhotonVision on {ref}`"Other Debian-Based Co-Processor Installation" <docs/advanced-installation/sw_install/other-coprocessors:Other Debian-Based Co-Processor Installation>`. As it mentions, this will require an internet connection so connecting the Raspberry Pi to an internet-connected router via an Ethernet cable will be the easiest solution. The pi must remain writable while you are following these steps!
|
||||
|
||||
Next, from the SSH terminal, run `sudo nano /home/pi/runCamera` then arrow down to the start of the exec line and press "Enter" to add a new line. Then add `#` before the exec command to comment it out. Then, arrow up to the new line and type `sleep 10000`. Hit "Ctrl + O" and then "Enter" to save the file. Finally press "Ctrl + X" to exit nano. Now, reboot the Romi by typing `sudo reboot`.
|
||||
:::..Temporary instructions explaining how to install the older version of PhotonVision on a Romi. Remove when no longer needed.
|
||||
:::{attention}
|
||||
The version of WPILibPi for the Romi is 2023.2.1, which is not compatible with the current version of PhotonVision. **If you are using WPILibPi 2023.2.1 on your Romi, you must install PhotonVision v2023.4.2 or earlier!**
|
||||
|
||||
To install a compatible version of PhotonVision, enter these commands in the SSH terminal connected to the Raspberry Pi. This will download and run the install script, which will intall PhotonVision on your Raspberry Pi and configure it to run at startup.
|
||||
|
||||
```bash
|
||||
$ wget https://git.io/JJrEP -O install.sh
|
||||
$ sudo chmod +x install.sh
|
||||
$ sudo ./install.sh -v 2023.4.2
|
||||
```
|
||||
The install script requires an internet connection, so connecting the Raspberry Pi to an internet-connected router via an Ethernet cable will be the easiest solution. The pi must remain writable while you are following these steps!
|
||||
:::
|
||||
:::..End of temporary instructions.
|
||||
|
||||
Next, from the SSH terminal, run `sudo nano /home/pi/runCamera` then arrow down to the start of the exec line and press "Enter" to add a new line. Then add `#` before the exec command to comment it out. Then, arrow up to the new line and type `sleep 10000`. Hit "Ctrl + O" and then "Enter" to save the file. Finally press "Ctrl + X" to exit nano. Now, reboot the Romi by typing `sudo reboot now`.
|
||||
|
||||
```{image} images/nano.png
|
||||
|
||||
```
|
||||
|
||||
After it reboots, you should be able to [locate the PhotonVision UI](https://photonvision.github.io/gloworm-docs/docs/quickstart/#finding-gloworm) at: `http://10.0.0.2:5800/`.
|
||||
After the Romi reboots, you should be able to open the PhotonVision UI at: [`http://10.0.0.2:5800/`](http://10.0.0.2:5800/). From here, you can adjust {ref}`Settings <docs/settings:Settings>` and configure {ref}`Pipelines <docs/pipelines/index:Pipelines>`.
|
||||
|
||||
:::{warning}
|
||||
In order for settings, logs, etc. to be saved / take effect, ensure that PhotonVision is in writable mode.
|
||||
:::
|
||||
|
||||
:::{attention}
|
||||
When using an older version of PhotonVision, the user interface and features may be different than what appears in the online documentation. The [Documentation](http://10.0.0.2:5800/#/docs) link in the User Interface will open a bundled version of the documentation that matches the PhotonVision version running on your coprocessor.
|
||||
:::
|
||||
|
||||
@@ -139,25 +139,7 @@ The `deploy` command is tested against Raspberry Pi coprocessors. Other similar
|
||||
|
||||
### Using PhotonLib Builds
|
||||
|
||||
The build process includes the following task:
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Linux
|
||||
|
||||
``./gradlew generateVendorJson``
|
||||
|
||||
.. tab-item:: macOS
|
||||
|
||||
``./gradlew generateVendorJson``
|
||||
|
||||
.. tab-item:: Windows (cmd)
|
||||
|
||||
``gradlew generateVendorJson``
|
||||
```
|
||||
|
||||
This generates a vendordep JSON of your local build at `photon-lib/build/generated/vendordeps/photonlib.json`.
|
||||
The build process automatically generates a vendordep JSON of your local build at `photon-lib/build/generated/vendordeps/photonlib.json`.
|
||||
|
||||
The photonlib source can be published to your local maven repository after building:
|
||||
|
||||
@@ -247,17 +229,15 @@ You can run one of the many built in examples straight from the command line, to
|
||||
|
||||
#### Running C++/Java
|
||||
|
||||
PhotonLib must first be published to your local maven repository, then the copy PhotonLib task will copy the generated vendordep json file into each example. After that, the simulateJava/simulateNative task can be used like a normal robot project. Robot simulation with attached debugger is technically possible by using simulateExternalJava and modifying the launch script it exports, though not yet supported.
|
||||
PhotonLib must first be published to your local maven repository. This will also copy the generated vendordep json file into each example. After that, the simulateJava/simulateNative task can be used like a normal robot project. Robot simulation with attached debugger is technically possible by using simulateExternalJava and modifying the launch script it exports, though not yet supported.
|
||||
|
||||
```
|
||||
~/photonvision$ ./gradlew publishToMavenLocal
|
||||
|
||||
~/photonvision$ cd photonlib-java-examples
|
||||
~/photonvision/photonlib-java-examples$ ./gradlew copyPhotonlib
|
||||
~/photonvision/photonlib-java-examples$ ./gradlew <example-name>:simulateJava
|
||||
|
||||
~/photonvision$ cd photonlib-cpp-examples
|
||||
~/photonvision/photonlib-cpp-examples$ ./gradlew copyPhotonlib
|
||||
~/photonvision/photonlib-cpp-examples$ ./gradlew <example-name>:simulateNative
|
||||
```
|
||||
|
||||
|
||||
@@ -82,6 +82,11 @@ const boardType = ref<CalibrationBoardTypes>(CalibrationBoardTypes.Charuco);
|
||||
const useOldPattern = ref(false);
|
||||
const tagFamily = ref<CalibrationTagFamilies>(CalibrationTagFamilies.Dict_4X4_1000);
|
||||
|
||||
// Emperical testing - with stack size limit of 1MB, we can handle at -least- 700k points
|
||||
const tooManyPoints = computed(
|
||||
() => useStateStore().calibrationData.imageCount * patternWidth.value * patternHeight.value > 700000
|
||||
);
|
||||
|
||||
const downloadCalibBoard = () => {
|
||||
const doc = new JsPDF({ unit: "in", format: "letter" });
|
||||
|
||||
@@ -413,12 +418,17 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
|
||||
</v-col>
|
||||
</v-row>
|
||||
<v-row>
|
||||
<v-col v-if="tooManyPoints" :cols="12">
|
||||
<v-banner rounded color="red" text-color="white" class="mt-3" icon="mdi-alert-circle-outline">
|
||||
Too many corners - finish calibration now!
|
||||
</v-banner>
|
||||
</v-col>
|
||||
<v-col :cols="6">
|
||||
<v-btn
|
||||
small
|
||||
color="secondary"
|
||||
style="width: 100%"
|
||||
:disabled="!settingsValid"
|
||||
:disabled="!settingsValid || tooManyPoints"
|
||||
@click="isCalibrating ? useCameraSettingsStore().takeCalibrationSnapshot() : startCalibration()"
|
||||
>
|
||||
<v-icon left class="calib-btn-icon"> {{ isCalibrating ? "mdi-camera" : "mdi-flag-outline" }} </v-icon>
|
||||
|
||||
2
photon-core/.gitignore
vendored
2
photon-core/.gitignore
vendored
@@ -11,5 +11,3 @@ photonvision/*
|
||||
photonvision_config/*
|
||||
photon-server/lib/*
|
||||
photon-server/package-lock.json
|
||||
|
||||
src/main/java/org/photonvision/PhotonVersion.java
|
||||
|
||||
@@ -65,9 +65,13 @@ dependencies {
|
||||
}
|
||||
|
||||
task writeCurrentVersion {
|
||||
def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"),
|
||||
versionString)
|
||||
doLast {
|
||||
def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$buildDir", "generated", "java", "org", "photonvision", "PhotonVersion.java"),
|
||||
versionString)
|
||||
}
|
||||
}
|
||||
// https://github.com/wpilibsuite/allwpilib/blob/main/wpilibj/build.gradle#L52
|
||||
sourceSets.main.java.srcDir "${buildDir}/generated/java/"
|
||||
|
||||
build.dependsOn writeCurrentVersion
|
||||
compileJava.dependsOn writeCurrentVersion
|
||||
|
||||
@@ -33,7 +33,7 @@ model {
|
||||
sources {
|
||||
cpp {
|
||||
source {
|
||||
srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp", "src/generate/native/cpp"
|
||||
srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp", "$buildDir/generated/native/cpp"
|
||||
include '**/*.cpp', '**/*.cc'
|
||||
}
|
||||
exportedHeaders {
|
||||
@@ -161,14 +161,12 @@ task generateVendorJson() {
|
||||
def read = photonlibFileInput.text
|
||||
.replace('${photon_version}', pubVersion)
|
||||
.replace('${frc_year}', frcYear)
|
||||
.replace('${wpilib_version}', wpilibVersion)
|
||||
photonlibFileOutput.text = read
|
||||
|
||||
outputs.upToDateWhen { false }
|
||||
}
|
||||
|
||||
build.mustRunAfter generateVendorJson
|
||||
publish.mustRunAfter generateVendorJson
|
||||
build.dependsOn generateVendorJson
|
||||
|
||||
task publishVendorJsonToLocalOutputs(type: Copy) {
|
||||
from photonlibFileOutput
|
||||
@@ -182,17 +180,69 @@ task publishVendorJsonToLocalOutputs(type: Copy) {
|
||||
publish.dependsOn it
|
||||
}
|
||||
|
||||
task writeCurrentVersion {
|
||||
def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"),
|
||||
versionString)
|
||||
versionFileIn = file("${rootDir}/shared/PhotonVersion.cpp.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "generate", "native", "cpp", "PhotonVersion.cpp"),
|
||||
versionString)
|
||||
task copyVendorJsonToExamples {
|
||||
outputs.upToDateWhen { false }
|
||||
jar.finalizedBy it
|
||||
}
|
||||
|
||||
build.mustRunAfter writeCurrentVersion
|
||||
cppHeadersZip.dependsOn writeCurrentVersion
|
||||
[
|
||||
"photonlib-cpp-examples",
|
||||
"photonlib-java-examples"
|
||||
].each { exampleFolder ->
|
||||
file("${rootDir}/${exampleFolder}")
|
||||
.listFiles()
|
||||
.findAll {
|
||||
return (it.isDirectory()
|
||||
&& !it.isHidden()
|
||||
&& !it.name.startsWith(".")
|
||||
&& it.toPath().resolve("build.gradle").toFile().exists())
|
||||
}
|
||||
.collect { it.name }
|
||||
.each { exampleVendordepFolder ->
|
||||
task "copyVendorJsonTo${exampleFolder}-${exampleVendordepFolder}"(type: Copy) {
|
||||
from photonlibFileOutput
|
||||
|
||||
into "${rootDir}/${exampleFolder}/${exampleVendordepFolder}/vendordeps/"
|
||||
outputs.upToDateWhen { false }
|
||||
copyVendorJsonToExamples.dependsOn it
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
clean {
|
||||
[
|
||||
"photonlib-cpp-examples",
|
||||
"photonlib-java-examples"
|
||||
].each { exampleFolder ->
|
||||
file("${rootDir}/${exampleFolder}")
|
||||
.listFiles()
|
||||
.findAll {
|
||||
return (it.isDirectory()
|
||||
&& !it.isHidden()
|
||||
&& !it.name.startsWith(".")
|
||||
&& it.toPath().resolve("build.gradle").toFile().exists())
|
||||
}
|
||||
.collect { it.name }
|
||||
.each { exampleVendordepFolder ->
|
||||
delete "${rootDir}/${exampleFolder}/${exampleVendordepFolder}/vendordeps/"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
task writeCurrentVersion {
|
||||
doLast {
|
||||
def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$buildDir", "generated", "java", "org", "photonvision", "PhotonVersion.java"),
|
||||
versionString)
|
||||
versionFileIn = file("${rootDir}/shared/PhotonVersion.cpp.in")
|
||||
writePhotonVersionFile(versionFileIn, Path.of("$buildDir", "generated", "native", "cpp", "PhotonVersion.cpp"),
|
||||
versionString)
|
||||
}
|
||||
}
|
||||
|
||||
// https://github.com/wpilibsuite/allwpilib/blob/main/wpilibj/build.gradle#L52
|
||||
sourceSets.main.java.srcDir "${buildDir}/generated/java/"
|
||||
compileJava.dependsOn writeCurrentVersion
|
||||
|
||||
// Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build.
|
||||
model {
|
||||
@@ -206,6 +256,7 @@ model {
|
||||
}
|
||||
it.binaries.all {
|
||||
it.tasks.withType(CppCompile) {
|
||||
it.dependsOn writeCurrentVersion
|
||||
it.dependsOn ":photon-targeting:generateProto"
|
||||
}
|
||||
}
|
||||
@@ -243,7 +294,7 @@ if (!project.hasProperty('copyOfflineArtifacts')) {
|
||||
tasks.named('cppSourcesZip') {
|
||||
dependsOn writeCurrentVersion
|
||||
|
||||
from("$projectDir/src/generate/native/cpp") {
|
||||
from("$buildDir/generated/native/cpp") {
|
||||
into '/'
|
||||
}
|
||||
}
|
||||
@@ -252,7 +303,6 @@ tasks.named('cppSourcesZip') {
|
||||
def zipBaseNameCombined = '_GROUP_org.photonvision_combinedcpp_ID_photonvision-combinedcpp_CLS'
|
||||
task combinedCppSourcesZip(type: Zip) {
|
||||
dependsOn(':photon-lib:cppSourcesZip', ':photon-targeting:cppSourcesZip')
|
||||
mustRunAfter(':photon-lib:cppHeadersZip', ':photon-targeting:cppHeadersZip')
|
||||
|
||||
destinationDirectory = file("$buildDir/outputs")
|
||||
archiveBaseName = zipBaseNameCombined
|
||||
@@ -270,7 +320,6 @@ task combinedCppSourcesZip(type: Zip) {
|
||||
}
|
||||
task combinedHeadersZip(type: Zip) {
|
||||
dependsOn(':photon-lib:cppHeadersZip', ':photon-targeting:cppHeadersZip')
|
||||
mustRunAfter(':photon-lib:cppHeadersZip', ':photon-targeting:cppHeadersZip')
|
||||
|
||||
destinationDirectory = file("$buildDir/outputs")
|
||||
archiveBaseName = zipBaseNameCombined
|
||||
@@ -316,7 +365,6 @@ def nativeTasks = wpilibTools.createExtractionTasks {
|
||||
|
||||
nativeTasks.addToSourceSetResources(sourceSets.test)
|
||||
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
|
||||
|
||||
@@ -27,6 +27,12 @@ class OpenCVHelp:
|
||||
|
||||
@staticmethod
|
||||
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
|
||||
"""Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
|
||||
three elements representing {x, y, z} in the EDN coordinate system.
|
||||
|
||||
:param translations: The translations to convert into a np.array
|
||||
"""
|
||||
|
||||
retVal: list[list] = []
|
||||
for translation in translations:
|
||||
trl = OpenCVHelp.translationNWUtoEDN(translation)
|
||||
@@ -38,6 +44,13 @@ class OpenCVHelp:
|
||||
|
||||
@staticmethod
|
||||
def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
|
||||
"""Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
|
||||
three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
|
||||
norm, and axis = rvec / norm)
|
||||
|
||||
:param rotation: The rotation to convert into a np.array
|
||||
"""
|
||||
|
||||
retVal: list[np.ndarray] = []
|
||||
rot = OpenCVHelp.rotationNWUtoEDN(rotation)
|
||||
rotVec = rot.getQuaternion().toRotationVector()
|
||||
@@ -88,6 +101,25 @@ class OpenCVHelp:
|
||||
def reorderCircular(
|
||||
elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
|
||||
) -> list[Any]:
|
||||
"""Reorders the list, optionally indexing backwards and wrapping around to the last element after
|
||||
the first, and shifting all indices in the direction of indexing.
|
||||
|
||||
e.g.
|
||||
|
||||
({1,2,3}, false, 1) == {2,3,1}
|
||||
|
||||
({1,2,3}, true, 0) == {1,3,2}
|
||||
|
||||
({1,2,3}, true, 1) == {3,2,1}
|
||||
|
||||
:param elements: list elements
|
||||
:param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
|
||||
:param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
|
||||
start at shiftStart, negated if backwards)
|
||||
|
||||
:returns: Reordered list
|
||||
"""
|
||||
|
||||
size = len(elements)
|
||||
reordered = []
|
||||
dir = -1 if backwards else 1
|
||||
@@ -100,18 +132,39 @@ class OpenCVHelp:
|
||||
|
||||
@staticmethod
|
||||
def translationEDNToNWU(trl: Translation3d) -> Translation3d:
|
||||
"""Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
|
||||
in EDN, this would be {0, -1, 0} in NWU.
|
||||
"""
|
||||
|
||||
return trl.rotateBy(EDN_TO_NWU)
|
||||
|
||||
@staticmethod
|
||||
def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
|
||||
"""Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
|
||||
in NWU, this would be {0, 0, 1} in EDN.
|
||||
"""
|
||||
|
||||
return -EDN_TO_NWU + (rot + EDN_TO_NWU)
|
||||
|
||||
@staticmethod
|
||||
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
|
||||
"""Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
|
||||
elements representing {x, y, z} in the EDN coordinate system.
|
||||
|
||||
:param tvecInput: The tvec to create a Translation3d from
|
||||
"""
|
||||
|
||||
return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
|
||||
|
||||
@staticmethod
|
||||
def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
|
||||
"""Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
|
||||
elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
|
||||
and axis = rvec / norm)
|
||||
|
||||
:param rvecInput: The rvec to create a Rotation3d from
|
||||
"""
|
||||
|
||||
return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
|
||||
|
||||
@staticmethod
|
||||
@@ -121,6 +174,33 @@ class OpenCVHelp:
|
||||
modelTrls: list[Translation3d],
|
||||
imagePoints: np.ndarray,
|
||||
) -> PnpResult | None:
|
||||
"""Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
|
||||
relative to the target is determined by the supplied 3d points of the target's model and their
|
||||
associated 2d points imaged by the camera. The supplied model translations must be relative to
|
||||
the target's pose.
|
||||
|
||||
For planar targets, there may be an alternate solution which is plausible given the 2d image
|
||||
points. This has an associated "ambiguity" which describes the ratio of reprojection error
|
||||
between the "best" and "alternate" solution.
|
||||
|
||||
This method is intended for use with individual AprilTags, and will not work unless 4 points
|
||||
are provided.
|
||||
|
||||
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
|
||||
:param distCoeffs: The camera distortion matrix in standard opencv form
|
||||
:param modelTrls: The translations of the object corners. These should have the object pose as
|
||||
their origin. These must come in a specific, pose-relative order (in NWU):
|
||||
|
||||
- Point 0: [0, -squareLength / 2, squareLength / 2]
|
||||
- Point 1: [0, squareLength / 2, squareLength / 2]
|
||||
- Point 2: [0, squareLength / 2, -squareLength / 2]
|
||||
- Point 3: [0, -squareLength / 2, -squareLength / 2]
|
||||
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
|
||||
should match the given object point translations.
|
||||
|
||||
:returns: The resulting transformation that maps the camera pose to the target pose and the
|
||||
ambiguity if an alternate solution is available.
|
||||
"""
|
||||
modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
|
||||
imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
|
||||
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
|
||||
@@ -130,6 +210,7 @@ class OpenCVHelp:
|
||||
best: Transform3d = Transform3d()
|
||||
|
||||
for tries in range(2):
|
||||
# calc rvecs/tvecs and associated reprojection error from image points
|
||||
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
|
||||
objectMat,
|
||||
imagePoints,
|
||||
@@ -138,6 +219,7 @@ class OpenCVHelp:
|
||||
flags=cv.SOLVEPNP_IPPE_SQUARE,
|
||||
)
|
||||
|
||||
# convert to wpilib coordinates
|
||||
best = Transform3d(
|
||||
OpenCVHelp.tVecToTranslation(tvecs[0]),
|
||||
OpenCVHelp.rVecToRotation(rvecs[0]),
|
||||
@@ -148,6 +230,7 @@ class OpenCVHelp:
|
||||
OpenCVHelp.rVecToRotation(rvecs[1]),
|
||||
)
|
||||
|
||||
# check if we got a NaN result
|
||||
if reprojectionError is not None and not math.isnan(
|
||||
reprojectionError[0, 0]
|
||||
):
|
||||
@@ -158,6 +241,7 @@ class OpenCVHelp:
|
||||
pt[0, 1] -= 0.001
|
||||
imagePoints[0] = pt
|
||||
|
||||
# solvePnP failed
|
||||
if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
|
||||
print("SolvePNP_Square failed!")
|
||||
return None
|
||||
@@ -186,6 +270,27 @@ class OpenCVHelp:
|
||||
modelTrls: list[Translation3d],
|
||||
imagePoints: np.ndarray,
|
||||
) -> PnpResult | None:
|
||||
"""Finds the transformation that maps the camera's pose to the origin of the supplied object. An
|
||||
"object" is simply a set of known 3d translations that correspond to the given 2d points. If,
|
||||
for example, the object translations are given relative to close-right corner of the blue
|
||||
alliance(the default origin), a camera-to-origin transformation is returned. If the
|
||||
translations are relative to a target's pose, a camera-to-target transformation is returned.
|
||||
|
||||
There must be at least 3 points to use this method. This does not return an alternate
|
||||
solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
|
||||
#solvePNP_SQUARE} instead.
|
||||
|
||||
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
|
||||
:param distCoeffs: The camera distortion matrix in standard opencv form
|
||||
:param objectTrls: The translations of the object corners, relative to the field.
|
||||
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
|
||||
should match the given object point translations.
|
||||
|
||||
:returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
|
||||
model points are supplied relative to the origin, this transformation brings the camera to
|
||||
the origin.
|
||||
"""
|
||||
|
||||
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
|
||||
|
||||
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
|
||||
|
||||
@@ -4,24 +4,38 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||
|
||||
|
||||
class RotTrlTransform3d:
|
||||
"""Represents a transformation that first rotates a pose around the origin, and then translates it."""
|
||||
|
||||
def __init__(
|
||||
self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
|
||||
):
|
||||
"""A rotation-translation transformation.
|
||||
|
||||
Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
|
||||
transform as if the origin was transformed by these components instead.
|
||||
|
||||
:param rot: The rotation component
|
||||
:param trl: The translation component
|
||||
"""
|
||||
self.rot = rot
|
||||
self.trl = trl
|
||||
|
||||
def inverse(self) -> Self:
|
||||
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
|
||||
invRot = -self.rot
|
||||
invTrl = -(self.trl.rotateBy(invRot))
|
||||
return type(self)(invRot, invTrl)
|
||||
|
||||
def getTransform(self) -> Transform3d:
|
||||
"""This transformation as a Transform3d (as if of the origin)"""
|
||||
return Transform3d(self.trl, self.rot)
|
||||
|
||||
def getTranslation(self) -> Translation3d:
|
||||
"""The translation component of this transformation"""
|
||||
return self.trl
|
||||
|
||||
def getRotation(self) -> Rotation3d:
|
||||
"""The rotation component of this transformation"""
|
||||
return self.rot
|
||||
|
||||
def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
|
||||
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
|
||||
|
||||
@classmethod
|
||||
def makeRelativeTo(cls, pose: Pose3d) -> Self:
|
||||
"""The rotation-translation transformation that makes poses in the world consider this pose as the
|
||||
new origin, or change the basis to this pose.
|
||||
|
||||
:param pose: The new origin
|
||||
"""
|
||||
return cls(pose.rotation(), pose.translation()).inverse()
|
||||
|
||||
@classmethod
|
||||
|
||||
@@ -8,14 +8,27 @@ from . import RotTrlTransform3d
|
||||
|
||||
|
||||
class TargetModel:
|
||||
"""Describes the 3d model of a target."""
|
||||
|
||||
def __init__(self):
|
||||
"""Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
|
||||
createCuboid, createSpheroid or create Arbitrary
|
||||
"""
|
||||
self.vertices: List[Translation3d] = []
|
||||
self.isPlanar = False
|
||||
self.isSpherical = False
|
||||
|
||||
@classmethod
|
||||
def createPlanar(cls, width: meters, height: meters) -> Self:
|
||||
"""Creates a rectangular, planar target model given the width and height. The model has four
|
||||
vertices:
|
||||
|
||||
- Point 0: [0, -width/2, -height/2]
|
||||
- Point 1: [0, width/2, -height/2]
|
||||
- Point 2: [0, width/2, height/2]
|
||||
- Point 3: [0, -width/2, height/2]
|
||||
"""
|
||||
|
||||
tm = cls()
|
||||
|
||||
tm.isPlanar = True
|
||||
@@ -30,6 +43,18 @@ class TargetModel:
|
||||
|
||||
@classmethod
|
||||
def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
|
||||
"""Creates a cuboid target model given the length, width, height. The model has eight vertices:
|
||||
|
||||
- Point 0: [length/2, -width/2, -height/2]
|
||||
- Point 1: [length/2, width/2, -height/2]
|
||||
- Point 2: [length/2, width/2, height/2]
|
||||
- Point 3: [length/2, -width/2, height/2]
|
||||
- Point 4: [-length/2, -width/2, height/2]
|
||||
- Point 5: [-length/2, width/2, height/2]
|
||||
- Point 6: [-length/2, width/2, -height/2]
|
||||
- Point 7: [-length/2, -width/2, -height/2]
|
||||
"""
|
||||
|
||||
tm = cls()
|
||||
verts = [
|
||||
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
|
||||
@@ -48,6 +73,20 @@ class TargetModel:
|
||||
|
||||
@classmethod
|
||||
def createSpheroid(cls, diameter: meters) -> Self:
|
||||
"""Creates a spherical target model which has similar dimensions regardless of its rotation. This
|
||||
model has four vertices:
|
||||
|
||||
- Point 0: [0, -radius, 0]
|
||||
- Point 1: [0, 0, -radius]
|
||||
- Point 2: [0, radius, 0]
|
||||
- Point 3: [0, 0, radius]
|
||||
|
||||
*Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
|
||||
like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
|
||||
are used for drawing the image of this sphere, but do not match the corners that will be
|
||||
published by photonvision.
|
||||
"""
|
||||
|
||||
tm = cls()
|
||||
|
||||
tm.isPlanar = False
|
||||
@@ -63,6 +102,14 @@ class TargetModel:
|
||||
|
||||
@classmethod
|
||||
def createArbitrary(cls, verts: List[Translation3d]) -> Self:
|
||||
"""Creates a target model from arbitrary 3d vertices. Automatically determines if the given
|
||||
vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
|
||||
vertices should define a non-intersecting contour.
|
||||
|
||||
:param vertices: Translations representing the vertices of this target model relative to its
|
||||
pose.
|
||||
"""
|
||||
|
||||
tm = cls()
|
||||
tm._common_construction(verts)
|
||||
|
||||
@@ -83,6 +130,12 @@ class TargetModel:
|
||||
self.vertices = verts
|
||||
|
||||
def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
|
||||
"""This target's vertices offset from its field pose.
|
||||
|
||||
Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
|
||||
Translation3d)} with this method.
|
||||
"""
|
||||
|
||||
basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
|
||||
|
||||
retVal = []
|
||||
@@ -94,6 +147,16 @@ class TargetModel:
|
||||
|
||||
@classmethod
|
||||
def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
|
||||
"""Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
|
||||
to the camera translation. This is used for spherical targets which should not have their
|
||||
projection change regardless of their own rotation.
|
||||
|
||||
:param tgtTrl: This target's translation
|
||||
:param cameraTrl: Camera's translation
|
||||
|
||||
:returns: This target's pose oriented to the camera
|
||||
"""
|
||||
|
||||
relCam = cameraTrl - tgtTrl
|
||||
orientToCam = Rotation3d(
|
||||
0.0,
|
||||
|
||||
@@ -11,6 +11,7 @@ class VisionEstimation:
|
||||
def getVisibleLayoutTags(
|
||||
visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
|
||||
) -> list[AprilTag]:
|
||||
"""Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
|
||||
retVal: list[AprilTag] = []
|
||||
for tag in visTags:
|
||||
id = tag.getFiducialId()
|
||||
@@ -30,12 +31,31 @@ class VisionEstimation:
|
||||
layout: AprilTagFieldLayout,
|
||||
tagModel: TargetModel,
|
||||
) -> PnpResult | None:
|
||||
"""Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
|
||||
field-to-camera transformation. If only one tag is visible, the result may have an alternate
|
||||
solution.
|
||||
|
||||
**Note:** The returned transformation is from the field origin to the camera pose!
|
||||
|
||||
With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
|
||||
|
||||
With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
|
||||
|
||||
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
|
||||
:param distCoeffs: The camera distortion matrix in standard opencv form
|
||||
:param visTags: The visible tags reported by PV. Non-tag targets are automatically excluded.
|
||||
:param tagLayout: The known tag layout on the field
|
||||
|
||||
:returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
|
||||
PnpResult} are present before utilizing them.
|
||||
"""
|
||||
if len(visTags) == 0:
|
||||
return None
|
||||
|
||||
corners: list[TargetCorner] = []
|
||||
knownTags: list[AprilTag] = []
|
||||
|
||||
# ensure these are AprilTags in our layout
|
||||
for tgt in visTags:
|
||||
id = tgt.getFiducialId()
|
||||
maybePose = layout.getTagPose(id)
|
||||
@@ -53,6 +73,7 @@ class VisionEstimation:
|
||||
|
||||
points = OpenCVHelp.cornersToPoints(corners)
|
||||
|
||||
# single-tag pnp
|
||||
if len(knownTags) == 1:
|
||||
camToTag = OpenCVHelp.solvePNP_Square(
|
||||
cameraMatrix, distCoeffs, tagModel.getVertices(), points
|
||||
@@ -74,6 +95,7 @@ class VisionEstimation:
|
||||
altReprojErr=camToTag.altReprojErr,
|
||||
)
|
||||
return result
|
||||
# multi-tag pnp
|
||||
else:
|
||||
objectTrls: list[Translation3d] = []
|
||||
for tag in knownTags:
|
||||
|
||||
@@ -9,6 +9,13 @@ PhotonPipelineResult_TYPE_STRING = (
|
||||
|
||||
|
||||
class NTTopicSet:
|
||||
"""This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
|
||||
It's split here so the sim and real-camera implementations can share a common implementation of
|
||||
the naming and registration of the NT content.
|
||||
|
||||
However, we do expect that the actual logic which fills out values in the entries will be
|
||||
different for sim vs. real camera
|
||||
"""
|
||||
|
||||
def __init__(self, tableName: str, cameraName: str) -> None:
|
||||
instance = nt.NetworkTableInstance.getDefault()
|
||||
|
||||
@@ -48,6 +48,10 @@ def setVersionCheckEnabled(enabled: bool):
|
||||
|
||||
class PhotonCamera:
|
||||
def __init__(self, cameraName: str):
|
||||
"""Constructs a PhotonCamera from the name of the camera.
|
||||
|
||||
:param cameraName: The nickname of the camera (found in the PhotonVision UI).
|
||||
"""
|
||||
instance = ntcore.NetworkTableInstance.getDefault()
|
||||
self._name = cameraName
|
||||
self._tableName = "photonvision"
|
||||
@@ -132,6 +136,14 @@ class PhotonCamera:
|
||||
return ret
|
||||
|
||||
def getLatestResult(self) -> PhotonPipelineResult:
|
||||
"""Returns the latest pipeline result. This is simply the most recent result Received via NT.
|
||||
Calling this multiple times will always return the most recent result.
|
||||
|
||||
Replaced by :meth:`.getAllUnreadResults` over getLatestResult, as this function can miss
|
||||
results, or provide duplicate ones!
|
||||
TODO implement the thing that will take this ones place...
|
||||
"""
|
||||
|
||||
self._versionCheck()
|
||||
|
||||
now = RobotController.getFPGATime()
|
||||
@@ -149,34 +161,85 @@ class PhotonCamera:
|
||||
return retVal
|
||||
|
||||
def getDriverMode(self) -> bool:
|
||||
"""Returns whether the camera is in driver mode.
|
||||
|
||||
:returns: Whether the camera is in driver mode.
|
||||
"""
|
||||
|
||||
return self._driverModeSubscriber.get()
|
||||
|
||||
def setDriverMode(self, driverMode: bool) -> None:
|
||||
"""Toggles driver mode.
|
||||
|
||||
:param driverMode: Whether to set driver mode.
|
||||
"""
|
||||
|
||||
self._driverModePublisher.set(driverMode)
|
||||
|
||||
def takeInputSnapshot(self) -> None:
|
||||
"""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.
|
||||
"""
|
||||
|
||||
self._inputSaveImgEntry.set(self._inputSaveImgEntry.get() + 1)
|
||||
|
||||
def takeOutputSnapshot(self) -> None:
|
||||
"""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.
|
||||
"""
|
||||
self._outputSaveImgEntry.set(self._outputSaveImgEntry.get() + 1)
|
||||
|
||||
def getPipelineIndex(self) -> int:
|
||||
"""Returns the active pipeline index.
|
||||
|
||||
:returns: The active pipeline index.
|
||||
"""
|
||||
|
||||
return self._pipelineIndexState.get(0)
|
||||
|
||||
def setPipelineIndex(self, index: int) -> None:
|
||||
"""Allows the user to select the active pipeline index.
|
||||
|
||||
:param index: The active pipeline index.
|
||||
"""
|
||||
self._pipelineIndexRequest.set(index)
|
||||
|
||||
def getLEDMode(self) -> VisionLEDMode:
|
||||
"""Returns the current LED mode.
|
||||
|
||||
:returns: The current LED mode.
|
||||
"""
|
||||
|
||||
mode = self._ledModeState.get()
|
||||
return VisionLEDMode(mode)
|
||||
|
||||
def setLEDMode(self, led: VisionLEDMode) -> None:
|
||||
"""Sets the LED mode.
|
||||
|
||||
:param led: The mode to set to.
|
||||
"""
|
||||
|
||||
self._ledModeRequest.set(led.value)
|
||||
|
||||
def getName(self) -> str:
|
||||
"""Returns the name of the camera. This will return the same value that was given to the
|
||||
constructor as cameraName.
|
||||
|
||||
:returns: The name of the camera.
|
||||
"""
|
||||
return self._name
|
||||
|
||||
def isConnected(self) -> bool:
|
||||
"""Returns whether the camera is connected and actively returning new data. Connection status is
|
||||
debounced.
|
||||
|
||||
:returns: True if the camera is actively sending frame data, false otherwise.
|
||||
"""
|
||||
|
||||
curHeartbeat = self._heartbeatEntry.get()
|
||||
now = Timer.getFPGATimestamp()
|
||||
|
||||
@@ -197,6 +260,8 @@ class PhotonCamera:
|
||||
|
||||
_lastVersionTimeCheck = Timer.getFPGATimestamp()
|
||||
|
||||
# Heartbeat entry is assumed to always be present. If it's not present, we
|
||||
# assume that a camera with that name was never connected in the first place.
|
||||
if not self._heartbeatEntry.exists():
|
||||
cameraNames = (
|
||||
self._cameraTable.getInstance().getTable(self._tableName).getSubTables()
|
||||
@@ -222,6 +287,7 @@ class PhotonCamera:
|
||||
True,
|
||||
)
|
||||
|
||||
# Check for connection status. Warn if disconnected.
|
||||
elif not self.isConnected():
|
||||
wpilib.reportWarning(
|
||||
f"PhotonVision coprocessor at path {self._path} is not sending new data.",
|
||||
@@ -229,8 +295,9 @@ class PhotonCamera:
|
||||
)
|
||||
|
||||
versionString = self.versionEntry.get(defaultValue="")
|
||||
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
|
||||
|
||||
# Check mdef UUID
|
||||
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
|
||||
remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
|
||||
|
||||
if not remoteUUID:
|
||||
|
||||
@@ -26,6 +26,10 @@ from .visionTargetSim import VisionTargetSim
|
||||
|
||||
|
||||
class PhotonCameraSim:
|
||||
"""A handle for simulating :class:`.PhotonCamera` values. Processing simulated targets through this
|
||||
class will change the associated PhotonCamera's results.
|
||||
"""
|
||||
|
||||
kDefaultMinAreaPx: float = 100
|
||||
|
||||
def __init__(
|
||||
@@ -35,6 +39,21 @@ class PhotonCameraSim:
|
||||
minTargetAreaPercent: float | None = None,
|
||||
maxSightRange: meters | None = None,
|
||||
):
|
||||
"""Constructs a handle for simulating :class:`.PhotonCamera` values. Processing simulated targets
|
||||
through this class will change the associated PhotonCamera's results.
|
||||
|
||||
By default, this constructor's camera has a 90 deg FOV with no simulated lag if props!
|
||||
By default, the minimum target area is 100 pixels and there is no maximum sight range unless both params are passed to override.
|
||||
|
||||
|
||||
:param camera: The camera to be simulated
|
||||
:param prop: Properties of this camera such as FOV and FPS
|
||||
:param minTargetAreaPercent: The minimum percentage(0 - 100) a detected target must take up of
|
||||
the camera's image to be processed. Match this with your contour filtering settings in the
|
||||
PhotonVision GUI.
|
||||
:param maxSightRangeMeters: Maximum distance at which the target is illuminated to your camera.
|
||||
Note that minimum target area of the image is separate from this.
|
||||
"""
|
||||
|
||||
self.minTargetAreaPercent: float = 0.0
|
||||
self.maxSightRange: float = 1.0e99
|
||||
@@ -103,22 +122,39 @@ class PhotonCameraSim:
|
||||
return self.videoSimFrameRaw
|
||||
|
||||
def canSeeTargetPose(self, camPose: Pose3d, target: VisionTargetSim) -> bool:
|
||||
"""Determines if this target's pose should be visible to the camera without considering its
|
||||
projected image points. Does not account for image area.
|
||||
|
||||
:param camPose: Camera's 3d pose
|
||||
:param target: Vision target containing pose and shape
|
||||
|
||||
:returns: If this vision target can be seen before image projection.
|
||||
"""
|
||||
|
||||
rel = CameraTargetRelation(camPose, target.getPose())
|
||||
return (
|
||||
(
|
||||
# target translation is outside of camera's FOV
|
||||
abs(rel.camToTargYaw.degrees())
|
||||
< self.prop.getHorizFOV().degrees() / 2.0
|
||||
and abs(rel.camToTargPitch.degrees())
|
||||
< self.prop.getVertFOV().degrees() / 2.0
|
||||
)
|
||||
and (
|
||||
# camera is behind planar target and it should be occluded
|
||||
not target.getModel().getIsPlanar()
|
||||
or abs(rel.targtoCamAngle.degrees()) < 90
|
||||
)
|
||||
# target is too far
|
||||
and rel.camToTarg.translation().norm() <= self.maxSightRange
|
||||
)
|
||||
|
||||
def canSeeCorner(self, points: np.ndarray) -> bool:
|
||||
"""Determines if all target points are inside the camera's image.
|
||||
|
||||
:param points: The target's 2d image points
|
||||
"""
|
||||
|
||||
assert points.shape[1] == 1
|
||||
assert points.shape[2] == 2
|
||||
for pt in points:
|
||||
@@ -130,51 +166,88 @@ class PhotonCameraSim:
|
||||
or y < 0
|
||||
or y > self.prop.getResHeight()
|
||||
):
|
||||
return False
|
||||
return False # point is outside of resolution
|
||||
|
||||
return True
|
||||
|
||||
def consumeNextEntryTime(self) -> float | None:
|
||||
"""Determine if this camera should process a new frame based on performance metrics and the time
|
||||
since the last update. This returns an Optional which is either empty if no update should occur
|
||||
or a Long of the timestamp in microseconds of when the frame which should be received by NT. If
|
||||
a timestamp is returned, the last frame update time becomes that timestamp.
|
||||
|
||||
:returns: Optional long which is empty while blocked or the NT entry timestamp in microseconds if
|
||||
ready
|
||||
"""
|
||||
# check if this camera is ready for another frame update
|
||||
now = int(wpilib.Timer.getFPGATimestamp() * 1e6)
|
||||
timestamp = 0
|
||||
iter = 0
|
||||
# prepare next latest update
|
||||
while now >= self.nextNtEntryTime:
|
||||
timestamp = int(self.nextNtEntryTime)
|
||||
frameTime = int(self.prop.estSecUntilNextFrame() * 1e6)
|
||||
self.nextNtEntryTime += frameTime
|
||||
|
||||
# if frame time is very small, avoid blocking
|
||||
iter += 1
|
||||
if iter > 50:
|
||||
timestamp = now
|
||||
self.nextNtEntryTime = now + frameTime
|
||||
break
|
||||
|
||||
# return the timestamp of the latest update
|
||||
if timestamp != 0:
|
||||
return timestamp
|
||||
|
||||
# or this camera isn't ready to process yet
|
||||
return None
|
||||
|
||||
def setMinTargetAreaPercent(self, areaPercent: float) -> None:
|
||||
"""The minimum percentage(0 - 100) a detected target must take up of the camera's image to be
|
||||
processed.
|
||||
"""
|
||||
self.minTargetAreaPercent = areaPercent
|
||||
|
||||
def setMinTargetAreaPixels(self, areaPx: float) -> None:
|
||||
"""The minimum number of pixels a detected target must take up in the camera's image to be
|
||||
processed.
|
||||
"""
|
||||
self.minTargetAreaPercent = areaPx / self.prop.getResArea() * 100.0
|
||||
|
||||
def setMaxSightRange(self, range: meters) -> None:
|
||||
"""Maximum distance at which the target is illuminated to your camera. Note that minimum target
|
||||
area of the image is separate from this.
|
||||
"""
|
||||
self.maxSightRange = range
|
||||
|
||||
def enableRawStream(self, enabled: bool) -> None:
|
||||
"""Sets whether the raw video stream simulation is enabled.
|
||||
|
||||
Note: This may increase loop times.
|
||||
"""
|
||||
self.videoSimRawEnabled = enabled
|
||||
raise Exception("Raw stream not implemented")
|
||||
|
||||
def enableDrawWireframe(self, enabled: bool) -> None:
|
||||
"""Sets whether a wireframe of the field is drawn to the raw video stream.
|
||||
|
||||
Note: This will dramatically increase loop times.
|
||||
"""
|
||||
self.videoSimWireframeEnabled = enabled
|
||||
raise Exception("Wireframe not implemented")
|
||||
|
||||
def setWireframeResolution(self, resolution: float) -> None:
|
||||
"""Sets the resolution of the drawn wireframe if enabled. Drawn line segments will be subdivided
|
||||
into smaller segments based on a threshold set by the resolution.
|
||||
|
||||
:param resolution: Resolution as a fraction(0 - 1) of the video frame's diagonal length in
|
||||
pixels
|
||||
"""
|
||||
self.videoSimWireframeResolution = resolution
|
||||
|
||||
def enableProcessedStream(self, enabled: bool) -> None:
|
||||
"""Sets whether the processed video stream simulation is enabled."""
|
||||
self.videoSimProcEnabled = enabled
|
||||
raise Exception("Processed stream not implemented")
|
||||
|
||||
@@ -187,25 +260,32 @@ class PhotonCameraSim:
|
||||
|
||||
targets.sort(key=distance, reverse=True)
|
||||
|
||||
# all targets visible before noise
|
||||
visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
|
||||
# all targets actually detected by camera (after noise)
|
||||
detectableTgts: list[PhotonTrackedTarget] = []
|
||||
|
||||
# basis change from world coordinates to camera coordinates
|
||||
camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
|
||||
|
||||
for tgt in targets:
|
||||
# pose isn't visible, skip to next
|
||||
if not self.canSeeTargetPose(cameraPose, tgt):
|
||||
continue
|
||||
|
||||
# find target's 3d corner points
|
||||
fieldCorners = tgt.getFieldVertices()
|
||||
isSpherical = tgt.getModel().getIsSpherical()
|
||||
if isSpherical:
|
||||
if isSpherical: # target is spherical
|
||||
model = tgt.getModel()
|
||||
# orient the model to the camera (like a sprite/decal) so it appears similar regardless of view
|
||||
fieldCorners = model.getFieldVertices(
|
||||
TargetModel.getOrientedPose(
|
||||
tgt.getPose().translation(), cameraPose.translation()
|
||||
)
|
||||
)
|
||||
|
||||
# project 3d target points into 2d image points
|
||||
imagePoints = OpenCVHelp.projectPoints(
|
||||
self.prop.getIntrinsics(),
|
||||
self.prop.getDistCoeffs(),
|
||||
@@ -213,9 +293,11 @@ class PhotonCameraSim:
|
||||
fieldCorners,
|
||||
)
|
||||
|
||||
# spherical targets need a rotated rectangle of their midpoints for visualization
|
||||
if isSpherical:
|
||||
center = OpenCVHelp.avgPoint(imagePoints)
|
||||
l: int = 0
|
||||
# reference point (left side midpoint)
|
||||
for i in range(4):
|
||||
if imagePoints[i, 0, 0] < imagePoints[l, 0, 0].x:
|
||||
l = i
|
||||
@@ -239,6 +321,7 @@ class PhotonCameraSim:
|
||||
for i in range(4):
|
||||
if i != t and i != l and i != b:
|
||||
r = i
|
||||
# create RotatedRect from midpoints
|
||||
rect = cv.RotatedRect(
|
||||
(center[0, 0], center[0, 1]),
|
||||
(
|
||||
@@ -247,16 +330,23 @@ class PhotonCameraSim:
|
||||
),
|
||||
-angles[r],
|
||||
)
|
||||
# set target corners to rect corners
|
||||
imagePoints = np.array([[p[0], p[1], p[2]] for p in rect.points()])
|
||||
|
||||
# save visible targets for raw video stream simulation
|
||||
visibleTgts.append((tgt, imagePoints))
|
||||
# estimate pixel noise
|
||||
noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
|
||||
# find the minimum area rectangle of target corners
|
||||
minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners)
|
||||
minAreaRectPts = minAreaRect.points()
|
||||
# find the (naive) 2d yaw/pitch
|
||||
centerPt = minAreaRect.center
|
||||
centerRot = self.prop.getPixelRot(centerPt)
|
||||
# find contour area
|
||||
areaPercent = self.prop.getContourAreaPercent(noisyTargetCorners)
|
||||
|
||||
# projected target can't be detected, skip to next
|
||||
if (
|
||||
not self.canSeeCorner(noisyTargetCorners)
|
||||
or not areaPercent >= self.minTargetAreaPercent
|
||||
@@ -265,6 +355,7 @@ class PhotonCameraSim:
|
||||
|
||||
pnpSim: PnpResult | None = None
|
||||
if tgt.fiducialId >= 0 and len(tgt.getFieldVertices()) == 4:
|
||||
# single AprilTag solvePNP
|
||||
pnpSim = OpenCVHelp.solvePNP_Square(
|
||||
self.prop.getIntrinsics(),
|
||||
self.prop.getDistCoeffs(),
|
||||
@@ -295,6 +386,7 @@ class PhotonCameraSim:
|
||||
|
||||
# Video streams disabled for now
|
||||
if self.videoSimRawEnabled:
|
||||
# TODO Video streams disabled for now port and uncomment when implemented
|
||||
# VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
|
||||
# cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
|
||||
# cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
|
||||
@@ -312,6 +404,7 @@ class PhotonCameraSim:
|
||||
|
||||
if len(visibleLayoutTags) > 1:
|
||||
usedIds = [tag.ID for tag in visibleLayoutTags]
|
||||
# sort target order sorts in ascending order by default
|
||||
usedIds.sort()
|
||||
pnpResult = VisionEstimation.estimateCamPosePNP(
|
||||
self.prop.getIntrinsics(),
|
||||
@@ -323,10 +416,16 @@ class PhotonCameraSim:
|
||||
if pnpResult is not None:
|
||||
multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
|
||||
|
||||
# put this simulated data to NT
|
||||
self.heartbeatCounter += 1
|
||||
now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
|
||||
return PhotonPipelineResult(
|
||||
metadata=PhotonPipelineMetadata(
|
||||
self.heartbeatCounter, int(latency * 1e6), 1000000
|
||||
self.heartbeatCounter,
|
||||
int(now_micros - latency * 1e6),
|
||||
int(now_micros),
|
||||
# Pretend like we heard a pong recently
|
||||
int(np.random.uniform(950, 1050)),
|
||||
),
|
||||
targets=detectableTgts,
|
||||
multitagResult=multiTagResults,
|
||||
@@ -335,6 +434,13 @@ class PhotonCameraSim:
|
||||
def submitProcessedFrame(
|
||||
self, result: PhotonPipelineResult, receiveTimestamp: float | None
|
||||
):
|
||||
"""Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp
|
||||
overrides :meth:`.PhotonPipelineResult.getTimestampSeconds` for more
|
||||
precise latency simulation.
|
||||
|
||||
:param result: The pipeline result to submit
|
||||
:param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency)
|
||||
"""
|
||||
if receiveTimestamp is None:
|
||||
receiveTimestamp = wpilib.Timer.getFPGATimestamp() * 1e6
|
||||
receiveTimestamp = int(receiveTimestamp)
|
||||
|
||||
@@ -11,7 +11,22 @@ from ..estimation import RotTrlTransform3d
|
||||
|
||||
|
||||
class SimCameraProperties:
|
||||
"""Calibration and performance values for this camera.
|
||||
|
||||
The resolution will affect the accuracy of projected(3d to 2d) target corners and similarly
|
||||
the severity of image noise on estimation(2d to 3d).
|
||||
|
||||
The camera intrinsics and distortion coefficients describe the results of calibration, and how
|
||||
to map between 3d field points and 2d image points.
|
||||
|
||||
The performance values (framerate/exposure time, latency) determine how often results should
|
||||
be updated and with how much latency in simulation. High exposure time causes motion blur which
|
||||
can inhibit target detection while moving. Note that latency estimation does not account for
|
||||
network latency and the latency reported will always be perfect.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Default constructor which is the same as {@link #PERFECT_90DEG}"""
|
||||
self.resWidth: int = -1
|
||||
self.resHeight: int = -1
|
||||
self.camIntrinsics: np.ndarray = np.zeros((3, 3)) # [3,3]
|
||||
@@ -38,14 +53,18 @@ class SimCameraProperties:
|
||||
fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
|
||||
fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
|
||||
|
||||
# assume no distortion
|
||||
newDistCoeffs = np.zeros((8, 1))
|
||||
|
||||
# assume centered principal point (pixels)
|
||||
cx = width / 2.0 - 0.5
|
||||
cy = height / 2.0 - 0.5
|
||||
|
||||
# use given fov to determine focal point (pixels)
|
||||
fx = cx / math.tan(fovWidth.radians() / 2.0)
|
||||
fy = cy / math.tan(fovHeight.radians() / 2.0)
|
||||
|
||||
# create camera intrinsics matrix
|
||||
newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
|
||||
|
||||
self.setCalibrationFromIntrinsics(
|
||||
@@ -65,6 +84,7 @@ class SimCameraProperties:
|
||||
self.camIntrinsics = newCamIntrinsics
|
||||
self.distCoeffs = newDistCoeffs
|
||||
|
||||
# left, right, up, and down view planes
|
||||
p = [
|
||||
Translation3d(
|
||||
1.0,
|
||||
@@ -110,16 +130,33 @@ class SimCameraProperties:
|
||||
self.errorStdDevPx = newErrorStdDevPx
|
||||
|
||||
def setFPS(self, fps: hertz):
|
||||
"""
|
||||
:param fps: The average frames per second the camera should process at. :strong:`Exposure time limits
|
||||
FPS if set!`
|
||||
"""
|
||||
|
||||
self.frameSpeed = max(1.0 / fps, self.exposureTime)
|
||||
|
||||
def setExposureTime(self, newExposureTime: seconds):
|
||||
"""
|
||||
:param newExposureTime: The amount of time the "shutter" is open for one frame. Affects motion
|
||||
blur. **Frame speed(from FPS) is limited to this!**
|
||||
"""
|
||||
|
||||
self.exposureTime = newExposureTime
|
||||
self.frameSpeed = max(self.frameSpeed, self.exposureTime)
|
||||
|
||||
def setAvgLatency(self, newAvgLatency: seconds):
|
||||
"""
|
||||
:param newAvgLatency: The average latency (from image capture to data published) in milliseconds
|
||||
a frame should have
|
||||
"""
|
||||
self.vgLatency = newAvgLatency
|
||||
|
||||
def setLatencyStdDev(self, newLatencyStdDev: seconds):
|
||||
"""
|
||||
:param latencyStdDevMs: The standard deviation in milliseconds of the latency
|
||||
"""
|
||||
self.latencyStdDev = newLatencyStdDev
|
||||
|
||||
def getResWidth(self) -> int:
|
||||
@@ -156,21 +193,43 @@ class SimCameraProperties:
|
||||
return self.latencyStdDev
|
||||
|
||||
def getContourAreaPercent(self, points: np.ndarray) -> float:
|
||||
"""The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the
|
||||
image.
|
||||
|
||||
:param points: Points of the contour
|
||||
"""
|
||||
|
||||
return cv.contourArea(cv.convexHull(points)) / self.getResArea() * 100.0
|
||||
|
||||
def getPixelYaw(self, pixelX: float) -> Rotation2d:
|
||||
"""The yaw from the principal point of this camera to the pixel x value. Positive values left."""
|
||||
fx = self.camIntrinsics[0, 0]
|
||||
# account for principal point not being centered
|
||||
cx = self.camIntrinsics[0, 2]
|
||||
xOffset = cx - pixelX
|
||||
return Rotation2d(fx, xOffset)
|
||||
|
||||
def getPixelPitch(self, pixelY: float) -> Rotation2d:
|
||||
"""The pitch from the principal point of this camera to the pixel y value. Pitch is positive down.
|
||||
|
||||
Note that this angle is naively computed and may be incorrect. See {@link
|
||||
#getCorrectedPixelRot(Point)}.
|
||||
"""
|
||||
|
||||
fy = self.camIntrinsics[1, 1]
|
||||
# account for principal point not being centered
|
||||
cy = self.camIntrinsics[1, 2]
|
||||
yOffset = cy - pixelY
|
||||
return Rotation2d(fy, -yOffset)
|
||||
|
||||
def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
|
||||
"""Finds the yaw and pitch to the given image point. Yaw is positive left, and pitch is positive
|
||||
down.
|
||||
|
||||
Note that pitch is naively computed and may be incorrect. See {@link
|
||||
#getCorrectedPixelRot(Point)}.
|
||||
"""
|
||||
|
||||
return Rotation3d(
|
||||
0.0,
|
||||
self.getPixelPitch(point[1]).radians(),
|
||||
@@ -178,6 +237,27 @@ class SimCameraProperties:
|
||||
)
|
||||
|
||||
def getCorrectedPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
|
||||
"""Gives the yaw and pitch of the line intersecting the camera lens and the given pixel
|
||||
coordinates on the sensor. Yaw is positive left, and pitch positive down.
|
||||
|
||||
The pitch traditionally calculated from pixel offsets do not correctly account for non-zero
|
||||
values of yaw because of perspective distortion (not to be confused with lens distortion)-- for
|
||||
example, the pitch angle is naively calculated as:
|
||||
|
||||
<pre>pitch = arctan(pixel y offset / focal length y)</pre>
|
||||
|
||||
However, using focal length as a side of the associated right triangle is not correct when the
|
||||
pixel x value is not 0, because the distance from this pixel (projected on the x-axis) to the
|
||||
camera lens increases. Projecting a line back out of the camera with these naive angles will
|
||||
not intersect the 3d point that was originally projected into this 2d pixel. Instead, this
|
||||
length should be:
|
||||
|
||||
<pre>focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))</pre>
|
||||
|
||||
:returns: Rotation3d with yaw and pitch of the line projected out of the camera from the given
|
||||
pixel (roll is zero).
|
||||
"""
|
||||
|
||||
fx = self.camIntrinsics[0, 0]
|
||||
cx = self.camIntrinsics[0, 2]
|
||||
xOffset = cx - point[0]
|
||||
@@ -191,11 +271,13 @@ class SimCameraProperties:
|
||||
return Rotation3d(0.0, pitch.radians(), yaw.radians())
|
||||
|
||||
def getHorizFOV(self) -> Rotation2d:
|
||||
# sum of FOV left and right principal point
|
||||
left = self.getPixelYaw(0)
|
||||
right = self.getPixelYaw(self.resWidth)
|
||||
return left - right
|
||||
|
||||
def getVertFOV(self) -> Rotation2d:
|
||||
# sum of FOV above and below principal point
|
||||
above = self.getPixelPitch(0)
|
||||
below = self.getPixelPitch(self.resHeight)
|
||||
return below - above
|
||||
@@ -208,9 +290,34 @@ class SimCameraProperties:
|
||||
def getVisibleLine(
|
||||
self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
|
||||
) -> typing.Tuple[float | None, float | None]:
|
||||
"""Determines where the line segment defined by the two given translations intersects the camera's
|
||||
frustum/field-of-vision, if at all.
|
||||
|
||||
The line is parametrized so any of its points <code>p = t * (b - a) + a</code>. This method
|
||||
returns these values of t, minimum first, defining the region of the line segment which is
|
||||
visible in the frustum. If both ends of the line segment are visible, this simply returns {0,
|
||||
1}. If, for example, point b is visible while a is not, and half of the line segment is inside
|
||||
the camera frustum, {0.5, 1} would be returned.
|
||||
|
||||
:param camRt: The change in basis from world coordinates to camera coordinates. See {@link
|
||||
RotTrlTransform3d#makeRelativeTo(Pose3d)}.
|
||||
:param a: The initial translation of the line
|
||||
:param b: The final translation of the line
|
||||
|
||||
:returns: A Pair of Doubles. The values may be null:
|
||||
|
||||
- {Double, Double} : Two parametrized values(t), minimum first, representing which
|
||||
segment of the line is visible in the camera frustum.
|
||||
- {Double, null} : One value(t) representing a single intersection point. For example,
|
||||
the line only intersects the intersection of two adjacent viewplanes.
|
||||
- {null, null} : No values. The line segment is not visible in the camera frustum.
|
||||
"""
|
||||
|
||||
# translations relative to the camera
|
||||
relA = camRt.applyTranslation(a)
|
||||
relB = camRt.applyTranslation(b)
|
||||
|
||||
# check if both ends are behind camera
|
||||
if relA.X() <= 0.0 and relB.X() <= 0.0:
|
||||
return (None, None)
|
||||
|
||||
@@ -221,6 +328,7 @@ class SimCameraProperties:
|
||||
aVisible = True
|
||||
bVisible = True
|
||||
|
||||
# check if the ends of the line segment are visible
|
||||
for normal in self.viewplanes:
|
||||
aVisibility = av.dot(normal)
|
||||
if aVisibility < 0:
|
||||
@@ -229,38 +337,54 @@ class SimCameraProperties:
|
||||
bVisibility = bv.dot(normal)
|
||||
if bVisibility < 0:
|
||||
bVisible = False
|
||||
# both ends are outside at least one of the same viewplane
|
||||
if aVisibility <= 0 and bVisibility <= 0:
|
||||
return (None, None)
|
||||
|
||||
# both ends are inside frustum
|
||||
if aVisible and bVisible:
|
||||
return (0.0, 1.0)
|
||||
|
||||
# parametrized (t=0 at a, t=1 at b) intersections with viewplanes
|
||||
intersections = [float("nan"), float("nan"), float("nan"), float("nan")]
|
||||
|
||||
# Optionally 3x1 vector
|
||||
ipts: typing.List[np.ndarray | None] = [None, None, None, None]
|
||||
|
||||
# find intersections
|
||||
for i, normal in enumerate(self.viewplanes):
|
||||
|
||||
# // we want to know the value of t when the line intercepts this plane
|
||||
# // parametrized: v = t * ab + a, where v lies on the plane
|
||||
# // we can find the projection of a onto the plane normal
|
||||
# // a_projn = normal.times(av.dot(normal) / normal.dot(normal));
|
||||
a_projn = (av.dot(normal) / normal.dot(normal)) * normal
|
||||
|
||||
# // this projection lets us determine the scalar multiple t of ab where
|
||||
# // (t * ab + a) is a vector which lies on the plane
|
||||
if abs(abv.dot(normal)) < 1.0e-5:
|
||||
continue
|
||||
intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn))
|
||||
|
||||
# // vector a to the viewplane
|
||||
apv = intersections[i] * abv
|
||||
# av + apv = intersection point
|
||||
intersectpt = av + apv
|
||||
ipts[i] = intersectpt
|
||||
|
||||
# // discard intersections outside the camera frustum
|
||||
for j in range(1, len(self.viewplanes)):
|
||||
if j == 0:
|
||||
continue
|
||||
oi = (i + j) % len(self.viewplanes)
|
||||
onormal = self.viewplanes[oi]
|
||||
# if the dot of the intersection point with any plane normal is negative, it is outside
|
||||
if intersectpt.dot(onormal) < 0:
|
||||
intersections[i] = float("nan")
|
||||
ipts[i] = None
|
||||
break
|
||||
|
||||
# // discard duplicate intersections
|
||||
if ipts[i] is None:
|
||||
continue
|
||||
|
||||
@@ -275,6 +399,7 @@ class SimCameraProperties:
|
||||
ipts[i] = None
|
||||
break
|
||||
|
||||
# determine visible segment (minimum and maximum t)
|
||||
inter1 = float("nan")
|
||||
inter2 = float("nan")
|
||||
for inter in intersections:
|
||||
@@ -284,6 +409,7 @@ class SimCameraProperties:
|
||||
else:
|
||||
inter2 = inter
|
||||
|
||||
# // two viewplane intersections
|
||||
if not math.isnan(inter2):
|
||||
max_ = max(inter1, inter2)
|
||||
min_ = min(inter1, inter2)
|
||||
@@ -292,16 +418,19 @@ class SimCameraProperties:
|
||||
if bVisible:
|
||||
max_ = 1
|
||||
return (min_, max_)
|
||||
# // one viewplane intersection
|
||||
elif not math.isnan(inter1):
|
||||
if aVisible:
|
||||
return (0, inter1)
|
||||
if bVisible:
|
||||
return (inter1, 1)
|
||||
return (inter1, None)
|
||||
# no intersections
|
||||
else:
|
||||
return (None, None)
|
||||
|
||||
def estPixelNoise(self, points: np.ndarray) -> np.ndarray:
|
||||
"""Returns these points after applying this camera's estimated noise."""
|
||||
assert points.shape[1] == 1, points.shape
|
||||
assert points.shape[2] == 2, points.shape
|
||||
if self.avgErrorPx == 0 and self.errorStdDevPx == 0:
|
||||
@@ -309,6 +438,7 @@ class SimCameraProperties:
|
||||
|
||||
noisyPts: list[list] = []
|
||||
for p in points:
|
||||
# // error pixels in random direction
|
||||
error = np.random.normal(self.avgErrorPx, self.errorStdDevPx, 1)[0]
|
||||
errorAngle = np.random.uniform(-math.pi, math.pi)
|
||||
noisyPts.append(
|
||||
@@ -324,16 +454,25 @@ class SimCameraProperties:
|
||||
return retval
|
||||
|
||||
def estLatency(self) -> seconds:
|
||||
"""
|
||||
:returns: Noisy estimation of a frame's processing latency
|
||||
"""
|
||||
|
||||
return max(
|
||||
float(np.random.normal(self.avgLatency, self.latencyStdDev, 1)[0]),
|
||||
0.0,
|
||||
)
|
||||
|
||||
def estSecUntilNextFrame(self) -> seconds:
|
||||
"""
|
||||
:returns: Estimate how long until the next frame should be processed in milliseconds
|
||||
"""
|
||||
# // exceptional processing latency blocks the next frame
|
||||
return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed)
|
||||
|
||||
@classmethod
|
||||
def PERFECT_90DEG(cls) -> typing.Self:
|
||||
"""960x720 resolution, 90 degree FOV, "perfect" lagless camera"""
|
||||
return cls()
|
||||
|
||||
@classmethod
|
||||
|
||||
@@ -15,7 +15,22 @@ from .visionTargetSim import VisionTargetSim
|
||||
|
||||
|
||||
class VisionSystemSim:
|
||||
"""A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
|
||||
running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
|
||||
this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
|
||||
should be updated periodically with the robot's current pose in order to publish the simulated
|
||||
camera target info.
|
||||
"""
|
||||
|
||||
def __init__(self, visionSystemName: str):
|
||||
"""A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
|
||||
running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
|
||||
this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
|
||||
should be updated periodically with the robot's current pose in order to publish the simulated
|
||||
camera target info.
|
||||
|
||||
:param visionSystemName: The specific identifier for this vision system in NetworkTables.
|
||||
"""
|
||||
self.dbgField: Field2d = Field2d()
|
||||
self.bufferLength: seconds = 1.5
|
||||
|
||||
@@ -32,12 +47,21 @@ class VisionSystemSim:
|
||||
wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField)
|
||||
|
||||
def getCameraSim(self, name: str) -> PhotonCameraSim | None:
|
||||
"""Get one of the simulated cameras."""
|
||||
return self.camSimMap.get(name, None)
|
||||
|
||||
def getCameraSims(self) -> list[PhotonCameraSim]:
|
||||
"""Get all the simulated cameras."""
|
||||
return [*self.camSimMap.values()]
|
||||
|
||||
def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> None:
|
||||
"""Adds a simulated camera to this vision system with a specified robot-to-camera transformation.
|
||||
The vision targets registered with this vision system simulation will be observed by the
|
||||
simulated :class:`.PhotonCamera`.
|
||||
|
||||
:param cameraSim: The camera simulation
|
||||
:param robotToCamera: The transform from the robot pose to the camera pose
|
||||
"""
|
||||
name = cameraSim.getCamera().getName()
|
||||
if name not in self.camSimMap:
|
||||
self.camSimMap[name] = cameraSim
|
||||
@@ -49,10 +73,15 @@ class VisionSystemSim:
|
||||
)
|
||||
|
||||
def clearCameras(self) -> None:
|
||||
"""Remove all simulated cameras from this vision system."""
|
||||
self.camSimMap.clear()
|
||||
self.camTrfMap.clear()
|
||||
|
||||
def removeCamera(self, cameraSim: PhotonCameraSim) -> bool:
|
||||
"""Remove a simulated camera from this vision system.
|
||||
|
||||
:returns: If the camera was present and removed
|
||||
"""
|
||||
name = cameraSim.getCamera().getName()
|
||||
if name in self.camSimMap:
|
||||
del self.camSimMap[name]
|
||||
@@ -65,6 +94,14 @@ class VisionSystemSim:
|
||||
cameraSim: PhotonCameraSim,
|
||||
time: seconds = wpilib.Timer.getFPGATimestamp(),
|
||||
) -> Transform3d | None:
|
||||
"""Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
|
||||
empty optional is returned.
|
||||
|
||||
:param cameraSim: The specific camera to get the robot-to-camera transform of
|
||||
:param timeSeconds: Timestamp in seconds of when the transform should be observed
|
||||
|
||||
:returns: The transform of this camera, or an empty optional if it is invalid
|
||||
"""
|
||||
if cameraSim in self.camTrfMap:
|
||||
trfBuffer = self.camTrfMap[cameraSim]
|
||||
sample = trfBuffer.sample(time)
|
||||
@@ -80,6 +117,13 @@ class VisionSystemSim:
|
||||
cameraSim: PhotonCameraSim,
|
||||
time: seconds = wpilib.Timer.getFPGATimestamp(),
|
||||
) -> Pose3d | None:
|
||||
"""Get a simulated camera's position on the field. If the requested camera is invalid, an empty
|
||||
optional is returned.
|
||||
|
||||
:param cameraSim: The specific camera to get the field pose of
|
||||
|
||||
:returns: The pose of this camera, or an empty optional if it is invalid
|
||||
"""
|
||||
robotToCamera = self.getRobotToCamera(cameraSim, time)
|
||||
if robotToCamera is None:
|
||||
return None
|
||||
@@ -93,6 +137,14 @@ class VisionSystemSim:
|
||||
def adjustCamera(
|
||||
self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
|
||||
) -> bool:
|
||||
"""Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or
|
||||
turret or some other mobile platform.
|
||||
|
||||
:param cameraSim: The simulated camera to change the relative position of
|
||||
:param robotToCamera: New transform from the robot to the camera
|
||||
|
||||
:returns: If the cameraSim was valid and transform was adjusted
|
||||
"""
|
||||
if cameraSim in self.camTrfMap:
|
||||
self.camTrfMap[cameraSim].addSample(
|
||||
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
|
||||
@@ -102,6 +154,7 @@ class VisionSystemSim:
|
||||
return False
|
||||
|
||||
def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
|
||||
"""Reset the transform history for this camera to just the current transform."""
|
||||
now = wpilib.Timer.getFPGATimestamp()
|
||||
|
||||
def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
|
||||
@@ -133,12 +186,30 @@ class VisionSystemSim:
|
||||
def addVisionTargets(
|
||||
self, targets: list[VisionTargetSim], targetType: str = "targets"
|
||||
) -> None:
|
||||
"""Adds targets on the field which your vision system is designed to detect. The {@link
|
||||
PhotonCamera}s simulated from this system will report the location of the camera relative to
|
||||
the subset of these targets which are visible from the given camera position.
|
||||
|
||||
:param targets: Targets to add to the simulated field
|
||||
:param type: Type of target (e.g. "cargo").
|
||||
"""
|
||||
|
||||
if targetType not in self.targetSets:
|
||||
self.targetSets[targetType] = targets
|
||||
else:
|
||||
self.targetSets[targetType] += targets
|
||||
|
||||
def addAprilTags(self, layout: AprilTagFieldLayout) -> None:
|
||||
"""Adds targets on the field which your vision system is designed to detect. The {@link
|
||||
PhotonCamera}s simulated from this system will report the location of the camera relative to
|
||||
the subset of these targets which are visible from the given camera position.
|
||||
|
||||
The AprilTags from this layout will be added as vision targets under the type "apriltag".
|
||||
The poses added preserve the tag layout's current alliance origin. If the tag layout's alliance
|
||||
origin is changed, these added tags will have to be cleared and re-added.
|
||||
|
||||
:param tagLayout: The field tag layout to get Apriltag poses and IDs from
|
||||
"""
|
||||
targets: list[VisionTargetSim] = []
|
||||
for tag in layout.getTags():
|
||||
tag_pose = layout.getTagPose(tag.ID)
|
||||
@@ -172,9 +243,15 @@ class VisionSystemSim:
|
||||
def getRobotPose(
|
||||
self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
|
||||
) -> Pose3d | None:
|
||||
"""Get the robot pose in meters saved by the vision system at this timestamp.
|
||||
|
||||
:param timestamp: Timestamp of the desired robot pose
|
||||
"""
|
||||
|
||||
return self.robotPoseBuffer.sample(timestamp)
|
||||
|
||||
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
|
||||
"""Clears all previous robot poses and sets robotPose at current time."""
|
||||
if type(robotPose) is Pose2d:
|
||||
robotPose = Pose3d(robotPose)
|
||||
assert type(robotPose) is Pose3d
|
||||
@@ -186,16 +263,23 @@ class VisionSystemSim:
|
||||
return self.dbgField
|
||||
|
||||
def update(self, robotPose: Pose2d | Pose3d) -> None:
|
||||
"""Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
|
||||
determine if a new frame should be submitted.
|
||||
|
||||
:param robotPoseMeters: The simulated robot pose in meters
|
||||
"""
|
||||
if type(robotPose) is Pose2d:
|
||||
robotPose = Pose3d(robotPose)
|
||||
assert type(robotPose) is Pose3d
|
||||
|
||||
# update vision targets on field
|
||||
for targetType, targets in self.targetSets.items():
|
||||
posesToAdd: list[Pose2d] = []
|
||||
for target in targets:
|
||||
posesToAdd.append(target.getPose().toPose2d())
|
||||
self.dbgField.getObject(targetType).setPoses(posesToAdd)
|
||||
|
||||
# save "real" robot poses over time
|
||||
now = wpilib.Timer.getFPGATimestamp()
|
||||
self.robotPoseBuffer.addSample(now, robotPose)
|
||||
self.dbgField.setRobotPose(robotPose.toPose2d())
|
||||
@@ -208,17 +292,22 @@ class VisionSystemSim:
|
||||
visTgtPoses2d: list[Pose2d] = []
|
||||
cameraPoses2d: list[Pose2d] = []
|
||||
processed = False
|
||||
# process each camera
|
||||
for camSim in self.camSimMap.values():
|
||||
# check if this camera is ready to process and get latency
|
||||
optTimestamp = camSim.consumeNextEntryTime()
|
||||
if optTimestamp is None:
|
||||
continue
|
||||
else:
|
||||
processed = True
|
||||
|
||||
# when this result "was" read by NT
|
||||
timestampNt = optTimestamp
|
||||
latency = camSim.prop.estLatency()
|
||||
# the image capture timestamp in seconds of this result
|
||||
timestampCapture = timestampNt * 1.0e-6 - latency
|
||||
|
||||
# use camera pose from the image capture timestamp
|
||||
lateRobotPose = self.getRobotPose(timestampCapture)
|
||||
robotToCamera = self.getRobotToCamera(camSim, timestampCapture)
|
||||
if lateRobotPose is None or robotToCamera is None:
|
||||
@@ -226,8 +315,11 @@ class VisionSystemSim:
|
||||
lateCameraPose = lateRobotPose + robotToCamera
|
||||
cameraPoses2d.append(lateCameraPose.toPose2d())
|
||||
|
||||
# process a PhotonPipelineResult with visible targets
|
||||
camResult = camSim.process(latency, lateCameraPose, allTargets)
|
||||
# publish this info to NT at estimated timestamp of receive
|
||||
camSim.submitProcessedFrame(camResult, timestampNt)
|
||||
# display debug results
|
||||
for tgt in camResult.getTargets():
|
||||
trf = tgt.getBestCameraToTarget()
|
||||
if trf == Transform3d():
|
||||
|
||||
@@ -6,7 +6,16 @@ from ..estimation.targetModel import TargetModel
|
||||
|
||||
|
||||
class VisionTargetSim:
|
||||
"""Describes a vision target located somewhere on the field that your vision system can detect."""
|
||||
|
||||
def __init__(self, pose: Pose3d, model: TargetModel, id: int = -1):
|
||||
"""Describes a fiducial tag located somewhere on the field that your vision system can detect.
|
||||
|
||||
:param pose: Pose3d of the tag in field-relative coordinates
|
||||
:param model: TargetModel which describes the shape of the target(tag)
|
||||
:param id: The ID of this fiducial tag
|
||||
"""
|
||||
|
||||
self.pose: Pose3d = pose
|
||||
self.model: TargetModel = model
|
||||
self.fiducialId: int = id
|
||||
@@ -47,4 +56,5 @@ class VisionTargetSim:
|
||||
return self.model
|
||||
|
||||
def getFieldVertices(self) -> list[Translation3d]:
|
||||
"""This target's vertices offset from its field pose."""
|
||||
return self.model.getFieldVertices(self.pose)
|
||||
|
||||
@@ -63,7 +63,6 @@ setup(
|
||||
"robotpy-apriltag<2026,>=2025.0.0b1",
|
||||
"robotpy-cscore<2026,>=2025.0.0b1",
|
||||
"pyntcore<2026,>=2025.0.0b1",
|
||||
"robotpy-opencv;platform_machine=='roborio'",
|
||||
"opencv-python;platform_machine!='roborio'",
|
||||
],
|
||||
description=descriptionStr,
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
package org.photonvision;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
|
||||
import static org.junit.jupiter.api.Assumptions.assumeTrue;
|
||||
import static org.photonvision.UnitTestUtils.waitForCondition;
|
||||
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
|
||||
|
||||
@@ -54,6 +55,9 @@ class PhotonCameraTest {
|
||||
@BeforeAll
|
||||
public static void load_wpilib() {
|
||||
WpilibLoader.loadLibraries();
|
||||
|
||||
// See #1574 - test flakey, disabled until we address this
|
||||
assumeTrue(false);
|
||||
}
|
||||
|
||||
@BeforeEach
|
||||
|
||||
@@ -28,6 +28,7 @@ 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 static org.junit.jupiter.api.Assertions.fail;
|
||||
import static org.junit.jupiter.api.Assumptions.assumeTrue;
|
||||
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
@@ -66,29 +67,27 @@ import org.photonvision.simulation.VisionSystemSim;
|
||||
import org.photonvision.simulation.VisionTargetSim;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
// See #1574 - flakey on windows and also linux, so commenting out until we bump wpilib
|
||||
class VisionSystemSimTest {
|
||||
private static final double kTrlDelta = 0.005;
|
||||
private static final double kRotDeltaDeg = 0.25;
|
||||
|
||||
NetworkTableInstance inst;
|
||||
|
||||
@BeforeAll
|
||||
public static void setUp() {
|
||||
WpilibLoader.loadLibraries();
|
||||
assertTrue(WpilibLoader.loadLibraries());
|
||||
|
||||
try {
|
||||
if (!PhotonTargetingJniLoader.load()) fail();
|
||||
assertTrue(PhotonTargetingJniLoader.load());
|
||||
} catch (UnsatisfiedLinkError | IOException e) {
|
||||
e.printStackTrace();
|
||||
fail(e);
|
||||
}
|
||||
|
||||
OpenCVHelp.forceLoadOpenCV();
|
||||
}
|
||||
|
||||
@BeforeEach
|
||||
public void init() {
|
||||
// // No version check for testing
|
||||
// PhotonCamera.setVersionCheckEnabled(false);
|
||||
// See #1574 - test flakey, disabled until we address this
|
||||
assumeTrue(false);
|
||||
}
|
||||
|
||||
@BeforeEach
|
||||
|
||||
2
photon-targeting/.gitignore
vendored
2
photon-targeting/.gitignore
vendored
@@ -9,5 +9,3 @@ build
|
||||
build/*
|
||||
photonvision/*
|
||||
photonvision_config/*
|
||||
|
||||
src/main/java/org/photonvision/PhotonVersion.java
|
||||
|
||||
@@ -68,7 +68,6 @@ public class WpilibLoader {
|
||||
"ntcorejni",
|
||||
"wpinetjni",
|
||||
"wpiHaljni",
|
||||
"wpi",
|
||||
"cscorejni",
|
||||
"apriltagjni");
|
||||
|
||||
|
||||
@@ -152,10 +152,9 @@ void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
|
||||
|
||||
wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server,
|
||||
int remote_port,
|
||||
std::chrono::milliseconds ping_delay,
|
||||
std::function<uint64_t()> timeProvider)
|
||||
std::chrono::milliseconds ping_delay)
|
||||
: m_logger(::ClientLoggerFunc),
|
||||
m_timeProvider(timeProvider),
|
||||
m_timeProvider(nt::Now),
|
||||
m_udp{},
|
||||
m_pingTimer{},
|
||||
m_serverIP{server},
|
||||
|
||||
@@ -97,10 +97,9 @@ void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n,
|
||||
// pong.client_time, pong.server_time);
|
||||
}
|
||||
|
||||
wpi::tsp::TimeSyncServer::TimeSyncServer(int port,
|
||||
std::function<uint64_t()> timeProvider)
|
||||
wpi::tsp::TimeSyncServer::TimeSyncServer(int port)
|
||||
: m_logger{::ServerLoggerFunc},
|
||||
m_timeProvider{timeProvider},
|
||||
m_timeProvider{nt::Now},
|
||||
m_udp{},
|
||||
m_port(port) {}
|
||||
|
||||
|
||||
@@ -74,11 +74,11 @@ class TimeSyncClient {
|
||||
|
||||
std::chrono::milliseconds m_loopDelay;
|
||||
|
||||
std::mutex m_offsetMutex;
|
||||
Metadata m_metadata;
|
||||
std::mutex m_offsetMutex{};
|
||||
Metadata m_metadata{};
|
||||
|
||||
// We only allow the most recent ping to stay alive, so only keep track of it
|
||||
TspPing m_lastPing;
|
||||
TspPing m_lastPing{};
|
||||
|
||||
// 30s is a reasonable guess
|
||||
frc::MedianFilter<int64_t> m_lastOffsets{30};
|
||||
@@ -90,8 +90,7 @@ class TimeSyncClient {
|
||||
|
||||
public:
|
||||
TimeSyncClient(std::string_view server, int remote_port,
|
||||
std::chrono::milliseconds ping_delay,
|
||||
std::function<uint64_t()> timeProvider = nt::Now);
|
||||
std::chrono::milliseconds ping_delay);
|
||||
|
||||
void Start();
|
||||
void Stop();
|
||||
|
||||
@@ -62,8 +62,7 @@ class TimeSyncServer {
|
||||
unsigned flags);
|
||||
|
||||
public:
|
||||
explicit TimeSyncServer(int port = 5810,
|
||||
std::function<uint64_t()> timeProvider = nt::Now);
|
||||
explicit TimeSyncServer(int port = 5810);
|
||||
|
||||
/**
|
||||
* Start listening for pings
|
||||
|
||||
@@ -18,12 +18,14 @@
|
||||
package wpiutil_extras;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.fail;
|
||||
import static org.junit.jupiter.api.Assumptions.assumeTrue;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import java.io.IOException;
|
||||
import org.junit.jupiter.api.AfterAll;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.common.hardware.Platform;
|
||||
import org.photonvision.jni.PhotonTargetingJniLoader;
|
||||
import org.photonvision.jni.QueuedFileLogger;
|
||||
import org.photonvision.jni.WpilibLoader;
|
||||
@@ -48,10 +50,10 @@ public class FileLoggerTest {
|
||||
|
||||
@Test
|
||||
public void smoketest() throws InterruptedException {
|
||||
var logger = new QueuedFileLogger("/var/log/kern.log");
|
||||
for (int i = 0; i < 100; i++) {
|
||||
Thread.sleep(1000);
|
||||
assumeTrue(Platform.isLinux());
|
||||
|
||||
var logger = new QueuedFileLogger("/var/log/kern.log");
|
||||
for (int i = 0; i < 1; i++) {
|
||||
for (var line : logger.getNewlines()) {
|
||||
System.out.println(" ->:" + line);
|
||||
}
|
||||
|
||||
@@ -14,8 +14,6 @@ wpi.maven.useDevelopment = false
|
||||
wpi.versions.wpilibVersion = "2025.1.1-beta-1"
|
||||
wpi.versions.wpimathVersion = "2025.1.1-beta-1"
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
|
||||
@@ -14,8 +14,6 @@ wpi.maven.useDevelopment = false
|
||||
wpi.versions.wpilibVersion = "2025.1.1-beta-1"
|
||||
wpi.versions.wpimathVersion = "2025.1.1-beta-1"
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
|
||||
@@ -22,9 +22,5 @@ spotless {
|
||||
}
|
||||
java {
|
||||
target "**/*.java"
|
||||
targetExclude("photon-core/src/main/java/org/photonvision/PhotonVersion.java")
|
||||
targetExclude("photon-lib/src/main/java/org/photonvision/PhotonVersion.java")
|
||||
}
|
||||
}
|
||||
|
||||
apply from: "examples.gradle"
|
||||
|
||||
@@ -14,8 +14,6 @@ wpi.maven.useDevelopment = false
|
||||
wpi.versions.wpilibVersion = "2025.1.1-beta-1"
|
||||
wpi.versions.wpimathVersion = "2025.1.1-beta-1"
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
|
||||
@@ -6,8 +6,6 @@ plugins {
|
||||
sourceCompatibility = JavaVersion.VERSION_11
|
||||
targetCompatibility = JavaVersion.VERSION_11
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||
|
||||
repositories {
|
||||
|
||||
@@ -6,8 +6,6 @@ plugins {
|
||||
sourceCompatibility = JavaVersion.VERSION_11
|
||||
targetCompatibility = JavaVersion.VERSION_11
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||
|
||||
wpi.maven.useDevelopment = true
|
||||
|
||||
@@ -2,8 +2,6 @@ plugins {
|
||||
id "com.diffplug.spotless" version "6.1.2"
|
||||
}
|
||||
|
||||
apply from: "examples.gradle"
|
||||
|
||||
allprojects {
|
||||
repositories {
|
||||
mavenCentral()
|
||||
@@ -24,7 +22,5 @@ spotless {
|
||||
}
|
||||
java {
|
||||
target "**/*.java"
|
||||
targetExclude("photon-core/src/main/java/org/photonvision/PhotonVersion.java")
|
||||
targetExclude("photon-lib/src/main/java/org/photonvision/PhotonVersion.java")
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,8 +6,6 @@ plugins {
|
||||
sourceCompatibility = JavaVersion.VERSION_11
|
||||
targetCompatibility = JavaVersion.VERSION_11
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||
|
||||
wpi.maven.useDevelopment = true
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
task copyPhotonlib() {
|
||||
doFirst {
|
||||
// Assume publish to maven local has just be run. Kinda curst
|
||||
|
||||
def vendorJsonSrc = new File("${rootDir}", "../photon-lib/build/generated/vendordeps/");
|
||||
def vendorJsonDst = new File("${projectDir}/vendordeps/");
|
||||
|
||||
delete(fileTree(vendorJsonDst) {
|
||||
exclude '.gitignore'
|
||||
})
|
||||
|
||||
println("Copying from ${vendorJsonSrc} to ${vendorJsonDst}")
|
||||
|
||||
copy {
|
||||
from vendorJsonSrc
|
||||
into vendorJsonDst
|
||||
}
|
||||
}
|
||||
outputs.upToDateWhen { false }
|
||||
}
|
||||
@@ -1,106 +0,0 @@
|
||||
apply plugin: 'cpp'
|
||||
apply plugin: 'google-test-test-suite'
|
||||
|
||||
apply from: "${rootDir}/shared/config.gradle"
|
||||
apply from: "${rootDir}/shared/javacommon.gradle"
|
||||
|
||||
wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
|
||||
|
||||
def nativeConfigName = 'wpilibNatives'
|
||||
def nativeConfig = configurations.create(nativeConfigName)
|
||||
|
||||
def nativeTasks = wpilibTools.createExtractionTasks {
|
||||
configurationName = nativeConfigName
|
||||
}
|
||||
|
||||
nativeTasks.addToSourceSetResources(sourceSets.main)
|
||||
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
|
||||
|
||||
// Windows specific functionality to export all symbols from a binary automatically
|
||||
nativeUtils {
|
||||
exportsConfigs {
|
||||
"${nativeName}" {}
|
||||
}
|
||||
}
|
||||
|
||||
model {
|
||||
components {
|
||||
"${nativeName}"(NativeLibrarySpec) {
|
||||
sources {
|
||||
cpp {
|
||||
source {
|
||||
srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp"
|
||||
include '**/*.cpp', '**/*.cc'
|
||||
}
|
||||
exportedHeaders {
|
||||
srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp"
|
||||
if (project.hasProperty('generatedHeaders')) {
|
||||
srcDir generatedHeaders
|
||||
}
|
||||
include "**/*.h"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
binaries.all {
|
||||
it.tasks.withType(CppCompile) {
|
||||
it.dependsOn generateProto
|
||||
}
|
||||
if(project.hasProperty('includePhotonTargeting')) {
|
||||
lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
|
||||
}
|
||||
}
|
||||
|
||||
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "opencv_shared")
|
||||
}
|
||||
}
|
||||
testSuites {
|
||||
"${nativeName}Test"(GoogleTestTestSuiteSpec) {
|
||||
for(NativeComponentSpec c : $.components) {
|
||||
if (c.name == nativeName) {
|
||||
testing c
|
||||
break
|
||||
}
|
||||
}
|
||||
sources {
|
||||
cpp {
|
||||
source {
|
||||
srcDirs 'src/test/native/cpp'
|
||||
include '**/*.cpp'
|
||||
}
|
||||
exportedHeaders {
|
||||
srcDirs 'src/test/native/include', "$buildDir/generated/source/proto/main/cpp"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
binaries.all {
|
||||
it.tasks.withType(CppCompile) {
|
||||
it.dependsOn generateProto
|
||||
}
|
||||
if(project.hasProperty('includePhotonTargeting')) {
|
||||
lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
|
||||
}
|
||||
}
|
||||
|
||||
nativeUtils.useRequiredLibrary(it, "cscore_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "googletest_static")
|
||||
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
|
||||
nativeUtils.useRequiredLibrary(it, "opencv_shared")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
apply from: "${rootDir}/shared/javacpp/publish.gradle"
|
||||
Reference in New Issue
Block a user