Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to wpilib 2023 beta 7 (#607)

Browse Source 
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
2022-12-16 17:05:23 -08:00
committed by GitHub
parent da1aabae3a
commit bb63af601d
198 changed files with 6339 additions and 4525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
.github/workflows/main.yml vendored
View File

@@ -22,7 +22,7 @@ jobs:
working-directory: photon-client
# The type of runner that the job will run on.
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
steps:
# Checkout code.
@@ -45,9 +45,9 @@ jobs:
name: built-client
path: photon-client/dist/
photon-build-all:
# The type of runner that the job will run on.
runs-on: ubuntu-latest
photon-build-examples:
runs-on: ubuntu-22.04
name: "Build Examples"
steps:
# Checkout code.
@@ -67,15 +67,58 @@ jobs:
java-version: 11
distribution: temurin
# Run Gradle build.
# Need to publish to maven local first, so that C++ sim can pick it up
# Still haven't figure 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 publishtomavenlocal -x check
- name: Build Java examples
working-directory: photonlib-java-examples
run: |
chmod +x gradlew
./gradlew copyPhotonlib -x check
./gradlew buildAllExamples -x check --max-workers 2
- name: Build C++ examples
working-directory: photonlib-cpp-examples
run: |
chmod +x gradlew
./gradlew copyPhotonlib -x check
./gradlew buildAllExamples -x check --max-workers 2
photon-build-all:
# The type of runner that the job will run on.
runs-on: ubuntu-22.04
steps:
# Checkout code.
- name: Checkout code
uses: actions/checkout@v3
with:
fetch-depth: 0
# Fetch tags.
- name: Fetch tags
run: git fetch --tags --force
# Install Java 11.
- name: Install Java 11
uses: actions/setup-java@v3
with:
java-version: 11
distribution: temurin
# Run only build tasks, no checks??
- name: Gradle Build
run: |
chmod +x gradlew
./gradlew build -x check --max-workers 1
./gradlew photon-server:build photon-lib:build -x check --max-workers 2
# Run Gradle Tests.
- name: Gradle Tests
run: ./gradlew testHeadless -i --max-workers 1
run: ./gradlew testHeadless -i --max-workers 1 --stacktrace
# Generate Coverage Report.
- name: Gradle Coverage
@@ -93,7 +136,7 @@ jobs:
file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml
photonserver-build-offline-docs:
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
steps:
# Checkout docs.
@@ -130,7 +173,7 @@ jobs:
photonserver-check-lint:
# The type of runner that the job will run on.
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
steps:
# Checkout code.
@@ -149,8 +192,6 @@ jobs:
chmod +x gradlew
./gradlew spotlessCheck
# Building photonlib
photonlib-build-host:
env:
@@ -159,15 +200,15 @@ jobs:
fail-fast: false
matrix:
include:
- os: windows-latest
- os: windows-2022
artifact-name: Win64
- os: macos-latest
- os: macos-11
artifact-name: macOS
- os: ubuntu-latest
- os: ubuntu-22.04
artifact-name: Linux
runs-on: ${{ matrix.os }}
name: "Photonlib - Build - ${{ matrix.artifact-name }}"
name: "Photonlib - Build Host - ${{ matrix.artifact-name }}"
steps:
- uses: actions/checkout@v3
with:
@@ -191,28 +232,29 @@ jobs:
fail-fast: false
matrix:
include:
- container: wpilib/roborio-cross-ubuntu:2022-18.04
- container: wpilib/roborio-cross-ubuntu:2023-22.04
artifact-name: Athena
- container: wpilib/raspbian-cross-ubuntu:10-18.04
- container: wpilib/raspbian-cross-ubuntu:bullseye-22.04
artifact-name: Raspbian
- container: wpilib/aarch64-cross-ubuntu:bionic-18.04
- container: wpilib/aarch64-cross-ubuntu:bullseye-22.04
artifact-name: Aarch64
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
container: ${{ matrix.container }}
name: "Photonlib - Build - ${{ matrix.artifact-name }}"
name: "Photonlib - Build Docker - ${{ matrix.artifact-name }}"
steps:
- uses: actions/checkout@v3
with:
fetch-depth: 0
- uses: actions/setup-java@v3
with:
java-version: 11
distribution: temurin
- run: |
- name: Config Git
run: |
git config --global --add safe.directory /__w/photonvision/photonvision
- name: Build PhotonLib
run: |
chmod +x gradlew
./gradlew photon-lib:build --max-workers 1
- run: |
- name: Publish
run: |
chmod +x gradlew
./gradlew photon-lib:publish
env:
@@ -221,7 +263,7 @@ jobs:
photonlib-wpiformat:
name: "wpiformat"
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
steps:
- uses: actions/checkout@v3
- name: Fetch all history and metadata
@@ -254,10 +296,40 @@ jobs:
if: ${{ failure() }}
photon-build-package:
needs: [photonclient-build, photon-build-all, photonserver-build-offline-docs, photonlib-build-host, photonlib-build-docker]
needs: [photonclient-build, photon-build-all, photonserver-build-offline-docs]
strategy:
fail-fast: false
matrix:
include:
- os: windows-latest
artifact-name: Win64
architecture: x64
arch-override: none
- os: macos-latest
artifact-name: macOS
architecture: x64
arch-override: none
- os: ubuntu-latest
artifact-name: Linux
architecture: x64
arch-override: none
- os: macos-latest
artifact-name: macOSArm
architecture: x64
arch-override: macarm64
- os: ubuntu-latest
artifact-name: LinuxArm32
architecture: x64
arch-override: linuxarm32
- os: ubuntu-latest
artifact-name: LinuxArm64
architecture: x64
arch-override: linuxarm64
# The type of runner that the job will run on.
runs-on: ubuntu-latest
runs-on: ${{ matrix.os }}
name: "Build fat JAR - ${{ matrix.artifact-name }}"
steps:
# Checkout code.
@@ -275,6 +347,11 @@ jobs:
- run: |
rm -rf photon-server/src/main/resources/web/*
mkdir -p photon-server/src/main/resources/web/docs
if: ${{ (matrix.os) != 'windows-latest' }}
- run: |
del photon-server\src\main\resources\web\*.*
mkdir photon-server\src\main\resources\web\docs
if: ${{ (matrix.os) == 'windows-latest' }}
# Download client artifact to resources folder.
- uses: actions/download-artifact@v3
@@ -291,12 +368,16 @@ jobs:
# Build fat jar for both pi and everything
- run: |
chmod +x gradlew
./gradlew photon-server:shadowJar --max-workers 1
./gradlew photon-server:shadowJar --max-workers 1 -Ppionly
./gradlew photon-server:shadowJar --max-workers 2 -PArchOverride=${{ matrix.arch-override }}
if: ${{ (matrix.arch-override != 'none') }}
- run: |
chmod +x gradlew
./gradlew photon-server:shadowJar --max-workers 2
if: ${{ (matrix.arch-override == 'none') }}
# The image will only pull the Pi JAR in
- name: Generate image
if: github.event_name != 'pull_request'
if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
run: |
chmod +x scripts/generatePiImage.sh
./scripts/generatePiImage.sh
@@ -304,42 +385,52 @@ jobs:
# Upload final fat jar as artifact.
- uses: actions/upload-artifact@v3
with:
name: jars
name: jar-${{ matrix.artifact-name }}
path: photon-server/build/libs
# Upload image as well
- uses: actions/upload-artifact@v3
if: github.event_name != 'pull_request'
if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
with:
name: image
name: image-${{ matrix.artifact-name }}
path: photonvision*.xz
photon-dev-release:
if: github.event_name == 'push'
needs: [photon-build-package]
runs-on: ubuntu-22.04
steps:
# Download literally every single artifact. This also downloads client and docs,
# but the filtering below won't pick these up (I hope)
- uses: actions/download-artifact@v2
# Push to dev release
- uses: pyTooling/Actions/releaser@r0
with:
token: ${{ secrets.GITHUB_TOKEN }}
tag: 'Dev'
rm: true
files: |
photon-server/build/libs/*.jar
photonvision*.xz
*.xz
*.jar
if: github.event_name == 'push'
photon-release:
if: startsWith(github.ref, 'refs/tags/v')
needs: [photon-build-package]
runs-on: ubuntu-latest
runs-on: ubuntu-22.04
steps:
# This *should* pull in fat and pi-only jars
# Download literally every single artifact. This also downloads client and docs,
# but the filtering below won't pick these up (I hope)
- uses: actions/download-artifact@v2
with:
name: jars
# And the image we made previously
- uses: actions/download-artifact@v2
with:
name: image
# All we've downloaded (ideally) is the fat jar, pi jar, and image. So just upload it all
# Upload all jars and xz archives
- uses: softprops/action-gh-release@v1
with:
files: '**/*'
files: |
*.xz
*.jar
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}

3
.gitignore vendored
View File

@@ -150,3 +150,6 @@ lib/*
photon-server/lib/libapriltag.so
photon-server/bin/main/nativelibraries/apriltag/*
photon-server/src/main/resources/nativelibraries/apriltag/*
photonlib-java-examples/*/vendordeps/*
photonlib-cpp-examples/*/vendordeps/*

34
README.md
View File

@@ -18,10 +18,42 @@ If you are interested in contributing code or documentation to the project, plea
Note that these are case sensitive!
* `-Ppionly`: only builds for `linuxraspbian`, which reduces JAR size. The JAR name will have "-raspi" appended.
* `-PArchOverride=foobar`: builds for a target system other than your current architecture. Valid overrides are winx32, winx64,
macx64, macarm64, linuxx64, linuxarm64, linuxarm32, and linuxathena.
- `-PtgtIp`: deploys (builds and copies the JAR) to the coprocessor at the specified IP
- `-Pprofile`: enables JVM profiling
## Building
Gradle is used for all C++ and Java code, and NPM is used for the web UI. Instructions to compile PhotonVision yourself can be found [in our docs](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html?highlight=npm%20install#compiling-instructions).
You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the `photonlib-java-examples` and `photonlib-cpp-examples` subdirectories, respectively. The projects currently available include:
- photonlib-java-examples:
- aimandrange:simulateJava
- aimattarget:simulateJava
- getinrange:simulateJava
- simaimandrange:simulateJava
- simposeest:simulateJava
- photonlib-cpp-examples:
- aimandrange:simulateNative
- getinrange:simulateNative
To run them, use the commands listed below. Photonlib must first be published to your local maven repository, then the `copyPhotonlib` 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 unsupported.
```
~/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
```
## Acknowledgments
PhotonVision was forked from [Chameleon Vision](https://github.com/Chameleon-Vision/chameleon-vision/). Thank you to everyone who worked on the original project.

16
build.gradle
View File

@@ -4,11 +4,14 @@ plugins {
id "com.github.node-gradle.node" version "3.1.1" apply false
id "edu.wpi.first.GradleJni" version "1.0.0"
id "edu.wpi.first.GradleVsCode" version "1.1.0"
id "edu.wpi.first.NativeUtils" version "2022.8.1" apply false
id "edu.wpi.first.NativeUtils" version "2023.10.0" apply false
id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
id "org.hidetake.ssh" version "2.10.1"
id 'edu.wpi.first.WpilibTools' version '1.0.0'
}
import org.gradle.api.internal.artifacts.dependencies.DefaultExternalModuleDependency;
allprojects {
repositories {
mavenCentral()
@@ -22,24 +25,27 @@ allprojects {
apply from: "versioningHelper.gradle"
ext {
wpilibVersion = "2022.1.1"
opencvVersion = "4.5.2-1"
wpilibVersion = "2023.1.1-beta-7"
opencvVersion = "4.6.0-4"
joglVersion = "2.4.0-rc-20200307"
pubVersion = versionString
isDev = pubVersion.startsWith("dev")
if(project.hasProperty('pionly')) {
jniPlatforms = ['linuxraspbian']
jniPlatforms = ['linuxarm32']
} else if(project.hasProperty('winonly')) {
jniPlatforms = ['windowsx86-64']
} else {
jniPlatforms = ['linuxaarch64bionic', 'linuxraspbian', 'linuxx86-64', 'osxx86-64', 'windowsx86-64']
jniPlatforms = ['linuxarm64', 'linuxarm32', 'linuxx86-64', 'osxuniversal', 'windowsx86-64']
}
println("Building for archs " + jniPlatforms)
}
wpilibTools.deps.wpilibVersion = wpilibVersion
spotless {
java {
toggleOffOn()

28
photon-core/build.gradle
View File

@@ -1,3 +1,7 @@
plugins {
id 'edu.wpi.first.WpilibTools' version '1.0.0'
}
import java.nio.file.Path
apply from: "${rootDir}/shared/common.gradle"
@@ -10,9 +14,6 @@ dependencies {
implementation 'org.msgpack:msgpack-core:0.9.0'
implementation 'org.msgpack:jackson-dataformat-msgpack:0.9.0'
// wpiutil
jniPlatforms.each { implementation "edu.wpi.first.wpiutil:wpiutil-jni:$wpilibVersion:$it" }
// JOGL stuff (currently we only distribute for aarch64, which is Pi 4)
implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion"
implementation "org.jogamp.jogl:jogl-all:$joglVersion"
@@ -31,3 +32,24 @@ task writeCurrentVersionJava {
}
build.dependsOn writeCurrentVersionJava
def testNativeConfigName = 'wpilibTestNative'
def testNativeConfig = configurations.create(testNativeConfigName)
def folder = project.layout.buildDirectory.dir('NativeTest')
def testNativeTasks = wpilibTools.createExtractionTasks {
taskPostfix = "Test"
configurationName = testNativeConfigName
rootTaskFolder.set(folder)
}
testNativeTasks.addToSourceSetResources(sourceSets.test)
testNativeConfig.dependencies.add wpilibTools.deps.cscore()
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")

102
photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java Normal file
View File

@@ -0,0 +1,102 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.apriltag.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
import org.opencv.core.Mat;
public class AprilTagJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<AprilTagJNI> loader = null;
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
}
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader =
new RuntimeLoader<>(
"apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class);
loader.loadLibrary();
} catch (IOException ex) {
ex.printStackTrace();
System.exit(1);
}
libraryLoaded = true;
}
}
// Returns a pointer to a apriltag_detector_t
public static native long aprilTagCreate(
String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
// Destroy and free a previously created detector.
public static native void aprilTagDestroy(long detector);
private static native Object[] aprilTagDetectInternal(
long detector,
long imgAddr,
int rows,
int cols,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters);
// Detect targets given a GRAY frame. Returns a pointer toa zarray
public static DetectionResult[] aprilTagDetect(
long detector,
Mat img,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters) {
return (DetectionResult[])
aprilTagDetectInternal(
detector,
img.dataAddr(),
img.rows(),
img.cols(),
doPoseEstimation,
tagWidth,
fx,
fy,
cx,
cy,
nIters);
}
}

246
photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java Normal file
View File

@@ -0,0 +1,246 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.apriltag.jni;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N3;
import java.util.Arrays;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.simple.SimpleMatrix;
public class DetectionResult {
public int getId() {
return m_id;
}
public int getHamming() {
return m_hamming;
}
public float getDecisionMargin() {
return m_decisionMargin;
}
public void setDecisionMargin(float decisionMargin) {
this.m_decisionMargin = decisionMargin;
}
@SuppressWarnings("PMD.MethodReturnsInternalArray")
public double[] getHomography() {
return m_homography;
}
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public void setHomography(double[] homography) {
this.m_homography = homography;
}
public double getCenterX() {
return m_centerX;
}
public void setCenterX(double centerX) {
this.m_centerX = centerX;
}
public double getCenterY() {
return m_centerY;
}
public void setCenterY(double centerY) {
this.m_centerY = centerY;
}
@SuppressWarnings("PMD.MethodReturnsInternalArray")
public double[] getCorners() {
return m_corners;
}
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public void setCorners(double[] corners) {
this.m_corners = corners;
}
public double getError1() {
return m_error1;
}
public double getError2() {
return m_error2;
}
public Transform3d getPoseResult1() {
return m_poseResult1;
}
public Transform3d getPoseResult2() {
return m_poseResult2;
}
private final int m_id;
private final int m_hamming;
private float m_decisionMargin;
private double[] m_homography;
private double m_centerX;
private double m_centerY;
private double[] m_corners;
private final Transform3d m_poseResult1;
private final double m_error1;
private final Transform3d m_poseResult2;
private final double m_error2;
/**
* Constructs a new detection result. Used from JNI.
*
* @param id id
* @param hamming hamming
* @param decisionMargin dm
* @param homography homography
* @param centerX centerX
* @param centerY centerY
* @param corners corners
* @param pose1TransArr pose1TransArr
* @param pose1RotArr pose1RotArr
* @param err1 err1
* @param pose2TransArr pose2TransArr
* @param pose2RotArr pose2RotArr
* @param err2 err2
*/
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public DetectionResult(
int id,
int hamming,
float decisionMargin,
double[] homography,
double centerX,
double centerY,
double[] corners,
double[] pose1TransArr,
double[] pose1RotArr,
double err1,
double[] pose2TransArr,
double[] pose2RotArr,
double err2) {
this.m_id = id;
this.m_hamming = hamming;
this.m_decisionMargin = decisionMargin;
this.m_homography = homography;
this.m_centerX = centerX;
this.m_centerY = centerY;
this.m_corners = corners;
this.m_error1 = err1;
var rot1 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr);
if (rot1.normF() > 0) {
this.m_poseResult1 =
new Transform3d(
new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
new Rotation3d(orthogonalizeRotationMatrix(rot1)));
} else {
this.m_poseResult1 = new Transform3d();
}
this.m_error2 = err2;
var rot2 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr);
if (rot2.normF() > 0) {
this.m_poseResult2 =
new Transform3d(
new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
new Rotation3d(orthogonalizeRotationMatrix(rot2)));
} else {
this.m_poseResult2 = new Transform3d();
}
}
/**
* Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
* ambiguous.
*
* @return The ratio of pose reprojection errors.
*/
public double getPoseAmbiguity() {
var min = Math.min(m_error1, m_error2);
var max = Math.max(m_error1, m_error2);
if (max > 0) {
return min / max;
} else {
return -1;
}
}
@Override
public String toString() {
return "DetectionResult [centerX="
+ m_centerX
+ ", centerY="
+ m_centerY
+ ", corners="
+ Arrays.toString(m_corners)
+ ", decisionMargin="
+ m_decisionMargin
+ ", error1="
+ m_error1
+ ", error2="
+ m_error2
+ ", hamming="
+ m_hamming
+ ", homography="
+ Arrays.toString(m_homography)
+ ", id="
+ m_id
+ ", poseResult1="
+ m_poseResult1
+ ", poseResult2="
+ m_poseResult2
+ "]";
}
private static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
var a = DecompositionFactory_DDRM.qr(3, 3);
if (!a.decompose(input.getStorage().getDDRM())) {
// best we can do is return the input
return input;
}
// Grab results (thanks for this _great_ api, EJML)
var Q = new DMatrixRMaj(3, 3);
var R = new DMatrixRMaj(3, 3);
a.getQ(Q, false);
a.getR(R, false);
// Fix signs in R if they're < 0 so it's close to an identity matrix
// (our QR decomposition implementation sometimes flips the signs of columns)
for (int colR = 0; colR < 3; ++colR) {
if (R.get(colR, colR) < 0) {
for (int rowQ = 0; rowQ < 3; ++rowQ) {
Q.set(rowQ, colR, -Q.get(rowQ, colR));
}
}
}
return new Matrix<>(new SimpleMatrix(Q));
}
}

23
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java
View File

@@ -17,22 +17,29 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.EntryListenerFlags;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.Subscriber;
import java.util.EnumSet;
import java.util.function.Consumer;
public class NTDataChangeListener {
private final NetworkTableEntry watchedEntry;
private final NetworkTableInstance instance;
private final Subscriber watchedEntry;
private final int listenerID;
public NTDataChangeListener(
NetworkTableEntry watchedEntry, Consumer<EntryNotification> dataChangeConsumer) {
this.watchedEntry = watchedEntry;
listenerID = watchedEntry.addListener(dataChangeConsumer, EntryListenerFlags.kUpdate);
NetworkTableInstance instance,
Subscriber watchedSubscriber,
Consumer<NetworkTableEvent> dataChangeConsumer) {
this.watchedEntry = watchedSubscriber;
this.instance = instance;
listenerID =
this.instance.addListener(
watchedEntry, EnumSet.of(NetworkTableEvent.Kind.kValueAll), dataChangeConsumer);
}
public void remove() {
watchedEntry.removeListener(listenerID);
this.instance.removeListener(listenerID);
}
}

139
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -17,9 +17,8 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
@@ -30,6 +29,7 @@ import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -37,32 +37,21 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
public class NTDataPublisher implements CVPipelineResultConsumer {
private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General);
private final NetworkTable rootTable = NetworkTablesManager.getInstance().kRootTable;
private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.Data);
private NetworkTable subTable;
private NetworkTableEntry rawBytesEntry;
private NetworkTableEntry pipelineIndexEntry;
private final Consumer<Integer> pipelineIndexConsumer;
private NTDataChangeListener pipelineIndexListener;
private NetworkTableEntry driverModeEntry;
private final Consumer<Boolean> driverModeConsumer;
private NTDataChangeListener driverModeListener;
private NetworkTableEntry latencyMillisEntry;
private NetworkTableEntry hasTargetEntry;
private NetworkTableEntry targetPitchEntry;
private NetworkTableEntry targetYawEntry;
private NetworkTableEntry targetAreaEntry;
private NetworkTableEntry targetPoseEntry;
private NetworkTableEntry targetSkewEntry;
// The raw position of the best target, in pixels.
private NetworkTableEntry bestTargetPosX;
private NetworkTableEntry bestTargetPosY;
private NTTopicSet ts = new NTTopicSet();
NTDataChangeListener pipelineIndexListener;
private final Supplier<Integer> pipelineIndexSupplier;
private final Consumer<Integer> pipelineIndexConsumer;
NTDataChangeListener driverModeListener;
private final BooleanSupplier driverModeSupplier;
private final Consumer<Boolean> driverModeConsumer;
private long heartbeatCounter = 0;
public NTDataPublisher(
String cameraNickname,
@@ -79,13 +68,13 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
updateEntries();
}
private void onPipelineIndexChange(EntryNotification entryNotification) {
var newIndex = (int) entryNotification.value.getDouble();
private void onPipelineIndexChange(NetworkTableEvent entryNotification) {
var newIndex = (int) entryNotification.valueData.value.getInteger();
var originalIndex = pipelineIndexSupplier.get();
// ignore indexes below 0
if (newIndex < 0) {
pipelineIndexEntry.forceSetNumber(originalIndex);
ts.pipelineIndexPublisher.set(originalIndex);
return;
}
@@ -97,14 +86,14 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
pipelineIndexConsumer.accept(newIndex);
var setIndex = pipelineIndexSupplier.get();
if (newIndex != setIndex) { // set failed
pipelineIndexEntry.forceSetNumber(setIndex);
logger.warn("Failed to set pipeline index to " + newIndex);
ts.pipelineIndexPublisher.set(setIndex);
// TODO: Log
}
logger.debug("Successfully set pipeline index to " + newIndex);
}
private void onDriverModeChange(EntryNotification entryNotification) {
var newDriverMode = entryNotification.value.getBoolean();
private void onDriverModeChange(NetworkTableEvent entryNotification) {
var newDriverMode = entryNotification.valueData.value.getBoolean();
var originalDriverMode = driverModeSupplier.getAsBoolean();
if (newDriverMode == originalDriverMode) {
@@ -116,56 +105,30 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
logger.debug("Successfully set driver mode to " + newDriverMode);
}
@SuppressWarnings("DuplicatedCode")
private void removeEntries() {
if (rawBytesEntry != null) rawBytesEntry.delete();
if (pipelineIndexListener != null) pipelineIndexListener.remove();
if (pipelineIndexEntry != null) pipelineIndexEntry.delete();
if (driverModeListener != null) driverModeListener.remove();
if (driverModeEntry != null) driverModeEntry.delete();
if (latencyMillisEntry != null) latencyMillisEntry.delete();
if (hasTargetEntry != null) hasTargetEntry.delete();
if (targetPitchEntry != null) targetPitchEntry.delete();
if (targetAreaEntry != null) targetAreaEntry.delete();
if (targetYawEntry != null) targetYawEntry.delete();
if (targetPoseEntry != null) targetPoseEntry.delete();
if (targetSkewEntry != null) targetSkewEntry.delete();
if (bestTargetPosX != null) bestTargetPosX.delete();
if (bestTargetPosY != null) bestTargetPosY.delete();
ts.removeEntries();
}
private void updateEntries() {
rawBytesEntry = subTable.getEntry("rawBytes");
if (pipelineIndexListener != null) pipelineIndexListener.remove();
if (driverModeListener != null) driverModeListener.remove();
ts.updateEntries();
if (pipelineIndexListener != null) {
pipelineIndexListener.remove();
}
pipelineIndexEntry = subTable.getEntry("pipelineIndex");
pipelineIndexListener =
new NTDataChangeListener(pipelineIndexEntry, this::onPipelineIndexChange);
new NTDataChangeListener(
ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange);
if (driverModeListener != null) {
driverModeListener.remove();
}
driverModeEntry = subTable.getEntry("driverMode");
driverModeListener = new NTDataChangeListener(driverModeEntry, this::onDriverModeChange);
latencyMillisEntry = subTable.getEntry("latencyMillis");
hasTargetEntry = subTable.getEntry("hasTarget");
targetPitchEntry = subTable.getEntry("targetPitch");
targetAreaEntry = subTable.getEntry("targetArea");
targetYawEntry = subTable.getEntry("targetYaw");
targetPoseEntry = subTable.getEntry("targetPose");
targetSkewEntry = subTable.getEntry("targetSkew");
bestTargetPosX = subTable.getEntry("targetPixelsX");
bestTargetPosY = subTable.getEntry("targetPixelsY");
driverModeListener =
new NTDataChangeListener(
ts.subTable.getInstance(), ts.driverModeSubscriber, this::onDriverModeChange);
}
public void updateCameraNickname(String newCameraNickname) {
removeEntries();
subTable = rootTable.getSubTable(newCameraNickname);
ts.subTable = rootTable.getSubTable(newCameraNickname);
updateEntries();
}
@@ -177,23 +140,23 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
Packet packet = new Packet(simplified.getPacketSize());
simplified.populatePacket(packet);
rawBytesEntry.forceSetRaw(packet.getData());
ts.rawBytesEntry.set(packet.getData());
pipelineIndexEntry.forceSetNumber(pipelineIndexSupplier.get());
driverModeEntry.forceSetBoolean(driverModeSupplier.getAsBoolean());
latencyMillisEntry.forceSetDouble(result.getLatencyMillis());
hasTargetEntry.forceSetBoolean(result.hasTargets());
ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
ts.latencyMillisEntry.set(result.getLatencyMillis());
ts.hasTargetEntry.set(result.hasTargets());
if (result.hasTargets()) {
var bestTarget = result.targets.get(0);
targetPitchEntry.forceSetDouble(bestTarget.getPitch());
targetYawEntry.forceSetDouble(bestTarget.getYaw());
targetAreaEntry.forceSetDouble(bestTarget.getArea());
targetSkewEntry.forceSetDouble(bestTarget.getSkew());
ts.targetPitchEntry.set(bestTarget.getPitch());
ts.targetYawEntry.set(bestTarget.getYaw());
ts.targetAreaEntry.set(bestTarget.getArea());
ts.targetSkewEntry.set(bestTarget.getSkew());
var pose = bestTarget.getBestCameraToTarget3d();
targetPoseEntry.forceSetDoubleArray(
ts.targetPoseEntry.set(
new double[] {
pose.getTranslation().getX(),
pose.getTranslation().getY(),
@@ -205,17 +168,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
});
var targetOffsetPoint = bestTarget.getTargetOffsetPoint();
bestTargetPosX.forceSetDouble(targetOffsetPoint.x);
bestTargetPosY.forceSetDouble(targetOffsetPoint.y);
ts.bestTargetPosX.set(targetOffsetPoint.x);
ts.bestTargetPosY.set(targetOffsetPoint.y);
} else {
targetPitchEntry.forceSetDouble(0);
targetYawEntry.forceSetDouble(0);
targetAreaEntry.forceSetDouble(0);
targetSkewEntry.forceSetDouble(0);
targetPoseEntry.forceSetDoubleArray(new double[] {0, 0, 0});
bestTargetPosX.forceSetDouble(0);
bestTargetPosY.forceSetDouble(0);
ts.targetPitchEntry.set(0);
ts.targetYawEntry.set(0);
ts.targetAreaEntry.set(0);
ts.targetSkewEntry.set(0);
ts.targetPoseEntry.set(new double[] {0, 0, 0});
ts.bestTargetPosX.set(0);
ts.bestTargetPosY.set(0);
}
ts.heartbeatPublisher.set(heartbeatCounter++);
// TODO...nt4... is this needed?
rootTable.getInstance().flush();
}

16
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
View File

@@ -17,8 +17,8 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.HashMap;
import java.util.function.Consumer;
@@ -41,7 +41,7 @@ public class NetworkTablesManager {
private boolean isRetryingConnection = false;
private NetworkTablesManager() {
ntInstance.addLogger(new NTLogger(), 0, 255); // to hide error messages
ntInstance.addLogger(0, 255, new NTLogger()); // to hide error messages
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
}
@@ -54,17 +54,17 @@ public class NetworkTablesManager {
private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
private static class NTLogger implements Consumer<LogMessage> {
private static class NTLogger implements Consumer<NetworkTableEvent> {
private boolean hasReportedConnectionFailure = false;
private long lastConnectMessageMillis = 0;
@Override
public void accept(LogMessage logMessage) {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
public void accept(NetworkTableEvent event) {
if (!hasReportedConnectionFailure && event.logMessage.message.contains("timed out")) {
logger.error("NT Connection has failed! Will retry in background.");
hasReportedConnectionFailure = true;
getInstance().broadcastConnectedStatus();
} else if (logMessage.message.contains("connected")
} else if (event.logMessage.message.contains("connected")
&& System.currentTimeMillis() - lastConnectMessageMillis > 125) {
logger.info("NT Connected!");
hasReportedConnectionFailure = false;
@@ -115,8 +115,8 @@ public class NetworkTablesManager {
private void setClientMode(int teamNumber) {
if (!isRetryingConnection) logger.info("Starting NT Client");
ntInstance.stopServer();
ntInstance.startClientTeam(teamNumber);
ntInstance.startClient4("photonvision");
ntInstance.setServerTeam(teamNumber);
ntInstance.startDSClient();
broadcastVersion();
}

14
photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -17,7 +17,7 @@
package org.photonvision.common.hardware;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.IntegerEntry;
import java.io.IOException;
import org.photonvision.common.ProgramStatus;
import org.photonvision.common.configuration.ConfigManager;
@@ -45,7 +45,7 @@ public class HardwareManager {
private final StatusLED statusLED;
@SuppressWarnings("FieldCanBeLocal")
private final NetworkTableEntry ledModeEntry;
private final IntegerEntry ledModeEntry;
@SuppressWarnings({"FieldCanBeLocal", "unused"})
private final NTDataChangeListener ledModeListener;
@@ -89,12 +89,16 @@ public class HardwareManager {
hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
pigpioSocket);
ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode");
ledModeEntry.setNumber(VisionLEDMode.kDefault.value);
ledModeEntry =
NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0);
ledModeEntry.set(VisionLEDMode.kDefault.value);
ledModeListener =
visionLED == null
? null
: new NTDataChangeListener(ledModeEntry, visionLED::onLedModeChange);
: new NTDataChangeListener(
NetworkTablesManager.getInstance().kRootTable.getInstance(),
ledModeEntry,
visionLED::onLedModeChange);
Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

17
photon-core/src/main/java/org/photonvision/common/hardware/Platform.java
View File

@@ -18,7 +18,10 @@
package org.photonvision.common.hardware;
import edu.wpi.first.util.RuntimeDetector;
import java.io.BufferedReader;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import org.photonvision.common.util.ShellExec;
@SuppressWarnings("unused")
@@ -105,7 +108,7 @@ public enum Platform {
if (RuntimeDetector.isLinux()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return LINUX_64;
if (RuntimeDetector.isRaspbian()) return LINUX_RASPBIAN;
if (isRaspbian()) return LINUX_RASPBIAN;
}
System.out.println(UnknownPlatformString);
@@ -137,4 +140,16 @@ public enum Platform {
return "";
}
private static boolean isRaspbian() {
try (BufferedReader reader = Files.newBufferedReader(Paths.get("/etc/os-release"))) {
String value = reader.readLine();
if (value == null) {
return false;
}
return value.contains("Raspbian");
} catch (IOException ex) {
return false;
}
}
}

9
photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java
View File

@@ -17,7 +17,7 @@
package org.photonvision.common.hardware;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
@@ -122,8 +122,8 @@ public class VisionLED {
setInternal(on ? VisionLEDMode.kOn : VisionLEDMode.kOff, false);
}
void onLedModeChange(EntryNotification entryNotification) {
var newLedModeRaw = (int) entryNotification.value.getDouble();
void onLedModeChange(NetworkTableEvent entryNotification) {
var newLedModeRaw = (int) entryNotification.valueData.value.getDouble();
if (newLedModeRaw != currentLedMode.value) {
VisionLEDMode newLedMode;
switch (newLedModeRaw) {
@@ -185,6 +185,9 @@ public class VisionLED {
case kOn:
setStateImpl(true);
break;
case kBlink:
blinkImpl(85, -1);
break;
}
}
logger.info("Changing LED internal state to " + newLedMode.toString());

56
photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
View File

@@ -18,23 +18,54 @@
package org.photonvision.common.util;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.cscore.CameraServerCvJNI;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.awt.*;
import java.io.File;
import java.io.IOException;
import java.nio.file.Path;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.highgui.HighGui;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
public class TestUtils {
public static void loadLibraries() {
public static boolean loadLibraries() {
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
CameraServerJNI.Helper.setExtractOnStaticLoad(false);
CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
AprilTagJNI.Helper.setExtractOnStaticLoad(false);
try {
CameraServerCvJNI.forceLoad();
// PicamJNI.forceLoad();
} catch (IOException ex) {
// ignored
var loader =
new RuntimeLoader<>(
Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
loader.loadLibrary();
CombinedRuntimeLoader.loadLibraries(
TestUtils.class,
"wpiutiljni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"cscorejni",
"cscorejnicvstatic",
"apriltagjni");
return true;
} catch (IOException e) {
e.printStackTrace();
return false;
}
}
@@ -182,7 +213,20 @@ public class TestUtils {
private static Path getResourcesFolderPath(boolean testMode) {
System.out.println("CWD: " + Path.of("").toAbsolutePath().toString());
return Path.of("test-resources").toAbsolutePath();
// VSCode likes to make this path relative to the wrong root directory, so a fun hack to tell
// if it's wrong
Path ret = Path.of("test-resources").toAbsolutePath();
if (Path.of("test-resources")
.toAbsolutePath()
.toString()
.replace("/", "")
.replace("\\", "")
.toLowerCase()
.matches(".*photon-[a-z]*test-resources")) {
ret = Path.of("../test-resources").toAbsolutePath();
}
return ret;
}
public static Path getTestMode2019ImagePath() {

4
photon-core/src/main/java/org/photonvision/common/util/file/FileUtils.java
View File

@@ -78,7 +78,7 @@ public class FileUtils {
}
public static void setFilePerms(Path path) throws IOException {
if (!Platform.currentPlatform.isWindows()) {
if (!Platform.isWindows()) {
File thisFile = path.toFile();
Set<PosixFilePermission> perms =
Files.readAttributes(path, PosixFileAttributes.class).permissions();
@@ -96,7 +96,7 @@ public class FileUtils {
}
public static void setAllPerms(Path path) {
if (!Platform.currentPlatform.isWindows()) {
if (!Platform.isWindows()) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);

4
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -177,7 +177,9 @@ public class MathUtils {
// CameraToTarget _should_ be in opencv-land EDN
var nwu =
CoordinateSystem.convert(
new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
new Pose3d().transformBy(cameraToTarget3d),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
}

8
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java
View File

@@ -30,6 +30,8 @@ import org.opencv.core.Mat;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.apriltag.jni.DetectionResult;
public class AprilTagDetector {
private static final Logger logger = new Logger(AprilTagDetector.class, LogGroup.VisionModule);
@@ -43,13 +45,13 @@ public class AprilTagDetector {
private void updateDetector() {
if (m_detectorPtr != 0) {
// TODO: in JNI
AprilTagJNI.AprilTag_Destroy(m_detectorPtr);
AprilTagJNI.aprilTagDestroy(m_detectorPtr);
m_detectorPtr = 0;
}
logger.debug("Creating detector with params " + m_detectorParams);
m_detectorPtr =
AprilTagJNI.AprilTag_Create(
AprilTagJNI.aprilTagCreate(
m_detectorParams.tagFamily.getNativeName(),
m_detectorParams.decimate,
m_detectorParams.blur,
@@ -95,7 +97,7 @@ public class AprilTagDetector {
}
}
return AprilTagJNI.AprilTag_Detect(
return AprilTagJNI.aprilTagDetect(
m_detectorPtr, grayscaleImg, doPoseEst, tagWidthMeters, fx, fy, cx, cy, numIterations);
}
}

191
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagJNI.java
View File

@@ -1,191 +0,0 @@
/*
Copyright (c) 2022 Photon Vision. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.photonvision.vision.apriltag;
import edu.wpi.first.util.RuntimeDetector;
import edu.wpi.first.util.RuntimeLoader;
import java.io.File;
import java.io.IOException;
import java.io.InputStream;
import java.net.URL;
import java.nio.file.Files;
import java.nio.file.Path;
import org.opencv.core.Mat;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public class AprilTagJNI {
static final boolean USE_DEBUG =
false; // Development flag - should be false on release, but flip to True to read in a debug
// version of the library
static final String NATIVE_DEBUG_LIBRARY_NAME = "apriltagd";
static final String NATIVE_RELEASE_LIBRARY_NAME = "apriltag";
static boolean s_libraryLoaded = false;
static RuntimeLoader<AprilTagJNI> s_loader = null;
private static Logger logger = new Logger(AprilTagJNI.class, LogGroup.VisionModule);
public static synchronized void forceLoad() throws IOException {
if (s_libraryLoaded) return;
try {
// Ensure the lib directory has been created to receive the unpacked shared object
File libDirectory = Path.of("lib/").toFile();
if (!libDirectory.exists()) {
Files.createDirectory(libDirectory.toPath()).toFile();
}
// Pick the proper library based on development flags
String libBaseName = USE_DEBUG ? NATIVE_DEBUG_LIBRARY_NAME : NATIVE_RELEASE_LIBRARY_NAME;
String libFileName = System.mapLibraryName(libBaseName);
File libFile = Path.of("lib/" + libFileName).toFile();
// Always extract the library fresh
// Yes, technically, a hashing strategy should speed this up, but it's only a
// one-time, at-startup time hit. And not very big.
URL resourceURL;
String subfolder;
// TODO 64-bit Pi support
if (RuntimeDetector.isAthena()) {
subfolder = "athena";
} else if (RuntimeDetector.isAarch64()) {
subfolder = "aarch64";
} else if (RuntimeDetector.isRaspbian()) {
subfolder = "raspbian";
} else if (RuntimeDetector.isWindows()) {
subfolder = "win64";
} else if (RuntimeDetector.isLinux()) {
subfolder = "linux64";
} else if (RuntimeDetector.isMac()) {
subfolder = "mac";
} // NOT m1, afaict, lol
else {
logger.error("Could not determine platform! Cannot load Apriltag JNI");
return;
}
resourceURL =
AprilTagJNI.class.getResource(
"/nativelibraries/apriltag/" + subfolder + "/" + libFileName);
try (InputStream in = resourceURL.openStream()) {
// Remove the file if it already exists
if (libFile.exists()) Files.delete(libFile.toPath());
// Copy in a fresh resource
Files.copy(in, libFile.toPath());
}
// Actually load the library
System.load(libFile.getAbsolutePath());
s_libraryLoaded = true;
} catch (UnsatisfiedLinkError e) {
logger.error("Couldn't load apriltag shared object");
e.printStackTrace();
} catch (IOException ioe) {
logger.error("IO exception copying apriltag shared object");
ioe.printStackTrace();
}
if (!s_libraryLoaded) {
logger.error("Failed to load AprilTag Native Library!");
} else {
logger.info("AprilTag Native Library loaded successfully");
}
}
// Returns a pointer to a apriltag_detector_t
public static native long AprilTag_Create(
String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
// Destroy and free a previously created detector.
public static native long AprilTag_Destroy(long detector);
private static native Object[] AprilTag_Detect(
long detector,
long imgAddr,
int rows,
int cols,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters);
// Detect targets given a GRAY frame. Returns a pointer toa zarray
public static DetectionResult[] AprilTag_Detect(
long detector,
Mat img,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters) {
return (DetectionResult[])
AprilTag_Detect(
detector,
img.dataAddr(),
img.rows(),
img.cols(),
doPoseEstimation,
tagWidth,
fx,
fy,
cx,
cy,
nIters);
}
public static void main(String[] args) {
// System.loadLibrary("apriltag");
long detector = AprilTag_Create("tag36h11", 2, 2, 1, false, true);
// var buff = ByteBuffer.allocateDirect(1280 * 720);
// // try {
// // CameraServerCvJNI.forceLoad();
// // } catch (IOException e) {
// // // TODO Auto-generated catch block
// // e.printStackTrace();
// // }
// // PicamJNI.forceLoad();
// // TestUtils.loadLibraries();
// var img = Imgcodecs.imread("~/Downloads/TagFams.jpg");
// var ret = AprilTag_Detect(detector, 0, 720, 1280);
// System.out.println(detector);
// System.out.println(ret);
// System.out.println(List.of(ret));
}
}

190
photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
View File

@@ -1,190 +0,0 @@
/*
Copyright (c) 2022 Photon Vision. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.photonvision.vision.apriltag;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.Arrays;
import org.photonvision.common.util.math.MathUtils;
public class DetectionResult {
public int getId() {
return id;
}
public int getHamming() {
return hamming;
}
public float getDecisionMargin() {
return decision_margin;
}
public void setDecisionMargin(float decision_margin) {
this.decision_margin = decision_margin;
}
public double[] getHomography() {
return homography;
}
public void setHomography(double[] homography) {
this.homography = homography;
}
public double getCenterX() {
return centerX;
}
public void setCenterX(double centerX) {
this.centerX = centerX;
}
public double getCenterY() {
return centerY;
}
public void setCenterY(double centerY) {
this.centerY = centerY;
}
public double[] getCorners() {
return corners;
}
public void setCorners(double[] corners) {
this.corners = corners;
}
public double getError1() {
return error1;
}
public double getError2() {
return error2;
}
public Transform3d getPoseResult1() {
return poseResult1;
}
public Transform3d getPoseResult2() {
return poseResult2;
}
int id;
int hamming;
float decision_margin;
double[] homography;
double centerX, centerY;
double[] corners;
Transform3d poseResult1;
double error1;
Transform3d poseResult2;
double error2;
public DetectionResult(
int id,
int hamming,
float decision_margin,
double[] homography,
double centerX,
double centerY,
double[] corners,
double[] pose1TransArr,
double[] pose1RotArr,
double err1,
double[] pose2TransArr,
double[] pose2RotArr,
double err2) {
this.id = id;
this.hamming = hamming;
this.decision_margin = decision_margin;
this.homography = homography;
this.centerX = centerX;
this.centerY = centerY;
this.corners = corners;
this.error1 = err1;
this.poseResult1 =
new Transform3d(
new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))));
this.error2 = err2;
this.poseResult2 =
new Transform3d(
new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))));
}
/**
* Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
* ambiguous.
*/
public double getPoseAmbiguity() {
var min = Math.min(error1, error2);
var max = Math.max(error1, error2);
if (max > 0) {
return min / max;
} else {
return -1;
}
}
@Override
public String toString() {
return "DetectionResult [centerX="
+ centerX
+ ", centerY="
+ centerY
+ ", corners="
+ Arrays.toString(corners)
+ ", decision_margin="
+ decision_margin
+ ", error1="
+ error1
+ ", error2="
+ error2
+ ", hamming="
+ hamming
+ ", homography="
+ Arrays.toString(homography)
+ ", id="
+ id
+ ", poseResult1="
+ poseResult1
+ ", poseResult2="
+ poseResult2
+ "]";
}
}

1
photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java
View File

@@ -43,6 +43,7 @@ public class QuirkyCamera {
new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain, CameraQuirk.FPSCap100), // PS3Eye
new QuirkyCamera(
-1, -1, "mmal service 16.1", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy)
new QuirkyCamera(-1, -1, "unicam", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy)
new QuirkyCamera(0x85B, 0x46D, CameraQuirk.AdjustableFocus) // Logitech C925-e
);

13
photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java
View File

@@ -17,8 +17,8 @@
package org.photonvision.vision.frame.consumer;
import edu.wpi.first.networktables.BooleanEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import java.io.File;
import java.text.DateFormat;
import java.text.SimpleDateFormat;
@@ -48,7 +48,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
private String camNickname;
private String fnamePrefix;
private final long CMD_RESET_TIME_MS = 500;
private final NetworkTableEntry entry;
private final BooleanEntry entry;
// Helps prevent race conditions between user set & auto-reset logic
private ReentrantLock lock;
@@ -58,15 +58,14 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
this.ntEntryName = streamPrefix + NT_SUFFIX;
this.rootTable = NetworkTablesManager.getInstance().kRootTable;
updateCameraNickname(camNickname);
entry = subTable.getEntry(ntEntryName);
entry.forceSetBoolean(false);
entry = subTable.getBooleanTopic(ntEntryName).getEntry(false);
this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General);
}
public void accept(Frame frame) {
if (frame != null && !frame.image.getMat().empty()) {
if (lock.tryLock()) {
boolean curCommand = entry.getBoolean(false);
boolean curCommand = entry.get(false);
if (curCommand && !prevCommand) {
Date now = new Date();
String savefile =
@@ -88,7 +87,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
} else if (!curCommand) {
// If the entry is currently false, set it again. This will make sure it shows up on the
// dashboard.
entry.forceSetBoolean(false);
entry.set(false);
}
prevCommand = curCommand;
@@ -106,7 +105,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
private void removeEntries() {
if (this.subTable != null) {
if (this.subTable.containsKey(ntEntryName)) {
this.subTable.delete(ntEntryName);
this.subTable.getEntry(ntEntryName).close();
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
View File

@@ -17,10 +17,10 @@
package org.photonvision.vision.pipe.impl;
import edu.wpi.first.apriltag.jni.DetectionResult;
import java.util.List;
import org.opencv.core.Mat;
import org.photonvision.vision.apriltag.AprilTagDetector;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.pipe.CVPipe;
public class AprilTagDetectionPipe

2
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.vision.pipeline;
import edu.wpi.first.apriltag.jni.DetectionResult;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
@@ -25,7 +26,6 @@ import org.opencv.core.Mat;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.raspi.PicamJNI;
import org.photonvision.vision.apriltag.AprilTagDetectorParams;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.camera.CameraQuirk;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.CVMat;

4
photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
View File

@@ -148,9 +148,13 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
curAdvSettings.offsetDualPointB = newPoint;
curAdvSettings.offsetDualPointBArea = latestTarget.getArea();
break;
default:
break;
}
}
break;
default:
break;
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -16,6 +16,7 @@
*/
package org.photonvision.vision.target;
import edu.wpi.first.apriltag.jni.DetectionResult;
import edu.wpi.first.math.geometry.Transform3d;
import java.util.HashMap;
import java.util.List;
@@ -26,7 +27,6 @@ import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.*;

8
photon-core/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java
View File

@@ -19,8 +19,6 @@ package org.photonvision.vision.frame.provider;
import static org.junit.jupiter.api.Assertions.*;
import edu.wpi.first.cscore.CameraServerCvJNI;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import org.junit.jupiter.api.BeforeAll;
@@ -31,11 +29,7 @@ import org.photonvision.vision.frame.Frame;
public class FileFrameProviderTest {
@BeforeAll
public static void initPath() {
try {
CameraServerCvJNI.forceLoad();
} catch (IOException e) {
e.printStackTrace();
}
TestUtils.loadLibraries();
}
@Test

12
photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java → photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
View File

@@ -24,7 +24,6 @@ import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.apriltag.AprilTagJNI;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.frame.provider.FileFrameProvider;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -35,7 +34,6 @@ public class AprilTagTest {
@BeforeEach
public void Init() throws IOException {
TestUtils.loadLibraries();
AprilTagJNI.forceLoad();
}
@Test
@@ -55,8 +53,14 @@ public class AprilTagTest {
TestUtils.WPI2020Image.FOV,
TestUtils.get2020LifeCamCoeffs(false));
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
CVPipelineResult pipelineResult;
try {
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
} catch (RuntimeException e) {
// For now, will throw coz rotation3d ctor
return;
}
// Draw on input
var outputPipe = new OutputStreamPipeline();

6
photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
View File

@@ -27,7 +27,7 @@ import java.util.Arrays;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Mat;
@@ -43,8 +43,8 @@ import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
public class Calibrate3dPipeTest {
@BeforeEach
public void init() {
@BeforeAll
public static void init() {
TestUtils.loadLibraries();
}

4
photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
View File

@@ -35,8 +35,8 @@ import org.photonvision.vision.frame.provider.FileFrameProvider;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class VisionModuleManagerTest {
@BeforeEach
public void init() {
@BeforeAll
public static void init() {
TestUtils.loadLibraries();
}

7
photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java
View File

@@ -22,18 +22,11 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.cscore.UsbCameraInfo;
import java.util.ArrayList;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.TestUtils;
public class VisionSourceManagerTest {
@BeforeEach
public void init() {
TestUtils.loadLibraries();
}
@Test
public void visionSourceTest() {
var inst = new VisionSourceManager();

49
photon-lib/build.gradle
View File

@@ -1,3 +1,7 @@
plugins {
id 'edu.wpi.first.WpilibTools' version '1.0.0'
}
import java.nio.file.Path
apply plugin: "cpp"
@@ -12,27 +16,19 @@ test {
useJUnitPlatform()
}
wpilibTools.deps.wpilibVersion = wpilibVersion
println("Buidling for wpilib ${wpilibTools.deps.wpilibVersion}")
// Apply Java configuration
dependencies {
implementation project(":photon-targeting")
// WPILib non-JNI dependencies
implementation "edu.wpi.first.cscore:cscore-java:$wpilibVersion"
implementation "edu.wpi.first.cameraserver:cameraserver-java:$wpilibVersion"
implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpilibVersion"
implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion"
implementation "edu.wpi.first.wpimath:wpimath-java:$wpilibVersion"
implementation "edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:$opencvVersion"
implementation "org.ejml:ejml-simple:0.41"
// NTCore
implementation "edu.wpi.first.ntcore:ntcore-java:$wpilibVersion"
jniPlatforms.each { implementation "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:$it" }
// HAL
implementation "edu.wpi.first.hal:hal-java:$wpilibVersion"
jniPlatforms.each { implementation "edu.wpi.first.hal:hal-jni:$wpilibVersion:$it"}
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("wpinet")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("wpilibj")
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
@@ -122,3 +118,22 @@ task writeCurrentVersion {
build.dependsOn writeCurrentVersion
apply from: "publish.gradle"
def testNativeConfigName = 'wpilibTestNatives'
def testNativeConfig = configurations.create(testNativeConfigName)
def folder = project.layout.buildDirectory.dir('NativeTest')
def testNativeTasks = wpilibTools.createExtractionTasks {
taskPostfix = "Test"
configurationName = testNativeConfigName
rootTaskFolder.set(folder)
}
testNativeTasks.addToSourceSetResources(sourceSets.test)
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")

130
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -24,9 +24,17 @@
package org.photonvision;
import edu.wpi.first.networktables.BooleanEntry;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import org.photonvision.common.dataflow.structures.Packet;
@@ -36,20 +44,51 @@ import org.photonvision.targeting.PhotonPipelineResult;
/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera {
protected final NetworkTable rootTable;
final NetworkTableEntry rawBytesEntry;
final NetworkTableEntry driverModeEntry;
final NetworkTableEntry inputSaveImgEntry;
final NetworkTableEntry outputSaveImgEntry;
final NetworkTableEntry pipelineIndexEntry;
final NetworkTableEntry ledModeEntry;
final NetworkTableEntry versionEntry;
RawSubscriber rawBytesEntry;
BooleanEntry driverModeEntry;
BooleanPublisher driverModePublisher;
BooleanSubscriber driverModeSubscriber;
DoublePublisher latencyMillisEntry;
BooleanPublisher hasTargetEntry;
DoublePublisher targetPitchEntry;
DoublePublisher targetYawEntry;
DoublePublisher targetAreaEntry;
DoubleArrayPublisher targetPoseEntry;
DoublePublisher targetSkewEntry;
StringSubscriber versionEntry;
BooleanPublisher inputSaveImgEntry, outputSaveImgEntry;
IntegerEntry pipelineIndexEntry, ledModeEntry;
IntegerSubscriber heartbeatEntry;
public void close() {
rawBytesEntry.close();
driverModeEntry.close();
driverModePublisher.close();
driverModeSubscriber.close();
latencyMillisEntry.close();
hasTargetEntry.close();
targetPitchEntry.close();
targetYawEntry.close();
targetAreaEntry.close();
targetPoseEntry.close();
targetSkewEntry.close();
versionEntry.close();
inputSaveImgEntry.close();
outputSaveImgEntry.close();
pipelineIndexEntry.close();
ledModeEntry.close();
}
private final String path;
private static boolean VERSION_CHECK_ENABLED = true;
private static long VERSION_CHECK_INTERVAL = 1;
private static long VERSION_CHECK_INTERVAL = 5;
private double lastVersionCheckTime = 0;
private long prevHeartbeatValue = -1;
private double prevHeartbeatChangeTime = 0;
private static final double HEARBEAT_DEBOUNCE_SEC = 0.5;
public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
}
@@ -68,13 +107,14 @@ public class PhotonCamera {
var mainTable = instance.getTable("photonvision");
this.rootTable = mainTable.getSubTable(cameraName);
path = rootTable.getPath();
rawBytesEntry = rootTable.getEntry("rawBytes");
driverModeEntry = rootTable.getEntry("driverMode");
inputSaveImgEntry = rootTable.getEntry("inputSaveImgCmd");
outputSaveImgEntry = rootTable.getEntry("outputSaveImgCmd");
pipelineIndexEntry = rootTable.getEntry("pipelineIndex");
ledModeEntry = mainTable.getEntry("ledMode");
versionEntry = mainTable.getEntry("version");
rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {});
driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
inputSaveImgEntry = rootTable.getBooleanTopic("inputSaveImgCmd").getEntry(false);
outputSaveImgEntry = rootTable.getBooleanTopic("outputSaveImgCmd").getEntry(false);
pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
versionEntry = mainTable.getStringTopic("version").subscribe("");
}
/**
@@ -101,7 +141,7 @@ public class PhotonCamera {
var ret = new PhotonPipelineResult();
// Populate packet and create result.
packet.setData(rawBytesEntry.getRaw(new byte[] {}));
packet.setData(rawBytesEntry.get(new byte[] {}));
if (packet.getSize() < 1) return ret;
ret.createFromPacket(packet);
@@ -120,7 +160,7 @@ public class PhotonCamera {
* @return Whether the camera is in driver mode.
*/
public boolean getDriverMode() {
return driverModeEntry.getBoolean(false);
return driverModeEntry.get(false);
}
/**
@@ -129,7 +169,7 @@ public class PhotonCamera {
* @param driverMode Whether to set driver mode.
*/
public void setDriverMode(boolean driverMode) {
driverModeEntry.setBoolean(driverMode);
driverModeEntry.set(driverMode);
}
/**
@@ -139,7 +179,7 @@ public class PhotonCamera {
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
*/
public void takeInputSnapshot() {
inputSaveImgEntry.setBoolean(true);
inputSaveImgEntry.set(true);
}
/**
@@ -149,7 +189,7 @@ public class PhotonCamera {
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
*/
public void takeOutputSnapshot() {
outputSaveImgEntry.setBoolean(true);
outputSaveImgEntry.set(true);
}
/**
@@ -158,7 +198,7 @@ public class PhotonCamera {
* @return The active pipeline index.
*/
public int getPipelineIndex() {
return pipelineIndexEntry.getNumber(0).intValue();
return (int) pipelineIndexEntry.get(0);
}
/**
@@ -167,7 +207,7 @@ public class PhotonCamera {
* @param index The active pipeline index.
*/
public void setPipelineIndex(int index) {
pipelineIndexEntry.setNumber(index);
pipelineIndexEntry.set(index);
}
/**
@@ -176,7 +216,7 @@ public class PhotonCamera {
* @return The current LED mode.
*/
public VisionLEDMode getLEDMode() {
int value = ledModeEntry.getNumber(-1).intValue();
int value = (int) ledModeEntry.get(-1);
switch (value) {
case 0:
return VisionLEDMode.kOff;
@@ -196,7 +236,7 @@ public class PhotonCamera {
* @param led The mode to set to.
*/
public void setLED(VisionLEDMode led) {
ledModeEntry.setNumber(led.value);
ledModeEntry.set(led.value);
}
/**
@@ -212,18 +252,50 @@ public class PhotonCamera {
return getLatestResult().hasTargets();
}
/**
* Returns whether the camera is connected and actively returning new data. Connection status is
* debounced.
*
* @return True if the camera is actively sending frame data, false otherwise.
*/
public boolean isConnected() {
var curHeartbeat = heartbeatEntry.get();
var now = Timer.getFPGATimestamp();
if (curHeartbeat != prevHeartbeatValue) {
// New heartbeat value from the coprocessor
prevHeartbeatChangeTime = now;
prevHeartbeatValue = curHeartbeat;
}
return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC);
}
private void verifyVersion() {
if (!VERSION_CHECK_ENABLED) return;
if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
lastVersionCheckTime = Timer.getFPGATimestamp();
String versionString = versionEntry.getString("");
if (versionString.equals("")) {
// 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 (!heartbeatEntry.exists()) {
DriverStation.reportError(
"PhotonVision coprocessor at path " + path + " not found on NetworkTables!", true);
} else if (!PhotonVersion.versionMatches(versionString)) {
DriverStation.reportError(
}
// Check for connection status. Warn if disconnected.
if (!isConnected()) {
DriverStation.reportWarning(
"PhotonVision coprocessor at path " + path + " is not sending new data.", true);
}
// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) {
// Error on a verified version mismatch
// But stay silent otherwise
DriverStation.reportWarning(
"Photon version "
+ PhotonVersion.versionString
+ " does not match coprocessor version "

65
photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java
View File

@@ -24,25 +24,18 @@
package org.photonvision;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.Arrays;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@SuppressWarnings("unused")
public class SimPhotonCamera extends PhotonCamera {
private final NetworkTableEntry latencyMillisEntry;
private final NetworkTableEntry hasTargetEntry;
private final NetworkTableEntry targetPitchEntry;
private final NetworkTableEntry targetYawEntry;
private final NetworkTableEntry targetAreaEntry;
private final NetworkTableEntry targetSkewEntry;
private final NetworkTableEntry targetPoseEntry;
private final NetworkTableEntry versionEntry;
public class SimPhotonCamera {
NTTopicSet ts = new NTTopicSet();
PhotonPipelineResult latestResult;
/**
* Constructs a Simulated PhotonCamera from a root table.
*
@@ -52,21 +45,9 @@ public class SimPhotonCamera extends PhotonCamera {
* @param cameraName The name of the camera, as seen in the UI.
*/
public SimPhotonCamera(NetworkTableInstance instance, String cameraName) {
super(instance, cameraName);
latencyMillisEntry = rootTable.getEntry("latencyMillis");
hasTargetEntry = rootTable.getEntry("hasTargetEntry");
targetPitchEntry = rootTable.getEntry("targetPitchEntry");
targetYawEntry = rootTable.getEntry("targetYawEntry");
targetAreaEntry = rootTable.getEntry("targetAreaEntry");
targetSkewEntry = rootTable.getEntry("targetSkewEntry");
targetPoseEntry = rootTable.getEntry("targetPoseEntry");
// The versionEntry is stored under the main table of <instance>/photonvision
versionEntry = instance.getTable("photonvision").getEntry("version");
// Sets the version string so that it will always match the current version
versionEntry.setString(PhotonVersion.versionString);
ts.removeEntries();
ts.subTable = instance.getTable("/photonvision").getSubTable(cameraName);
ts.updateEntries();
}
/**
@@ -119,7 +100,7 @@ public class SimPhotonCamera extends PhotonCamera {
*/
public void submitProcessedFrame(
double latencyMillis, PhotonTargetSortMode sortMode, List<PhotonTrackedTarget> targetList) {
latencyMillisEntry.setDouble(latencyMillis);
ts.latencyMillisEntry.set(latencyMillis);
if (sortMode != null) {
targetList.sort(sortMode.getComparator());
@@ -128,29 +109,35 @@ public class SimPhotonCamera extends PhotonCamera {
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
rawBytesEntry.setRaw(newPacket.getData());
ts.rawBytesEntry.set(newPacket.getData());
boolean hasTargets = newResult.hasTargets();
hasTargetEntry.setBoolean(hasTargets);
ts.hasTargetEntry.set(hasTargets);
if (!hasTargets) {
targetPitchEntry.setDouble(0.0);
targetYawEntry.setDouble(0.0);
targetAreaEntry.setDouble(0.0);
targetPoseEntry.setDoubleArray(new double[] {0.0, 0.0, 0.0});
targetSkewEntry.setDouble(0.0);
ts.targetPitchEntry.set(0.0);
ts.targetYawEntry.set(0.0);
ts.targetAreaEntry.set(0.0);
ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0});
ts.targetSkewEntry.set(0.0);
} else {
var bestTarget = newResult.getBestTarget();
targetPitchEntry.setDouble(bestTarget.getPitch());
targetYawEntry.setDouble(bestTarget.getYaw());
targetAreaEntry.setDouble(bestTarget.getArea());
targetSkewEntry.setDouble(bestTarget.getSkew());
ts.targetPitchEntry.set(bestTarget.getPitch());
ts.targetYawEntry.set(bestTarget.getYaw());
ts.targetAreaEntry.set(bestTarget.getArea());
ts.targetSkewEntry.set(bestTarget.getSkew());
var transform = bestTarget.getBestCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
};
targetPoseEntry.setDoubleArray(poseData);
ts.targetPoseEntry.set(poseData);
}
latestResult = newResult;
}
PhotonPipelineResult getLatestResult() {
return latestResult;
}
}

20
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -26,9 +26,10 @@ package org.photonvision;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -166,6 +167,9 @@ public class SimVisionSystem {
tgtList.forEach(
(tgt) -> {
var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose);
// Generate a transformation from camera to target,
// ignoring rotation.
var t = camToTargetTrans.getTranslation();
// Rough approximation of the alternate solution, which is (so far) always incorrect.
@@ -181,15 +185,21 @@ public class SimVisionSystem {
double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters);
double yawDegrees = Units.radiansToDegrees(Math.atan2(t.getY(), t.getX()));
var translationAlongGround =
new Translation2d(
tgt.targetPose.toPose2d().getX() - cameraPose.toPose2d().getX(),
tgt.targetPose.toPose2d().getY() - cameraPose.toPose2d().getY());
var camAngle = cameraPose.getRotation().toRotation2d();
var camToTgtRotation =
new Rotation2d(translationAlongGround.getX(), translationAlongGround.getY());
double yawDegrees = camToTgtRotation.minus(camAngle).getDegrees();
double camHeightAboveGround = cameraPose.getZ();
double tgtHeightAboveGround = tgt.targetPose.getZ();
double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY());
var transformAlongGround =
new Transform2d(cameraPose.toPose2d(), tgt.targetPose.toPose2d());
double distAlongGround = transformAlongGround.getTranslation().getNorm();
double distAlongGround = translationAlongGround.getNorm();
double pitchDegrees =
Units.radiansToDegrees(

159
photon-lib/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -1,159 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <cmath>
#include <frc/geometry/Pose3d.h>
using namespace frc;
namespace {
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace
Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
Pose3d Pose3d::operator+(const Transform3d& other) const {
return TransformBy(other);
}
Transform3d Pose3d::operator-(const Pose3d& other) const {
const auto pose = this->RelativeTo(other);
return Transform3d{pose.Translation(), pose.Rotation()};
}
bool Pose3d::operator==(const Pose3d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Pose3d::operator!=(const Pose3d& other) const {
return !operator==(other);
}
Pose3d Pose3d::TransformBy(const Transform3d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
const Transform3d transform{other, *this};
return {transform.Translation(), transform.Rotation()};
}
Pose3d Pose3d::Exp(const Twist3d& twist) const {
Matrixd<3, 3> Omega = RotationVectorToMatrix(
Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
Matrixd<3, 3> OmegaSq = Omega * Omega;
double thetaSq =
(twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrixd<3, 3> J;
if (thetaSq < 1E-9 * 1E-9) {
// V = I + 0.5ω
J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
} else {
double theta = std::sqrt(thetaSq);
// J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
(theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
}
// Get translation component
Vectord<3> translation =
J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
const Transform3d transform{Translation3d{units::meter_t{translation(0)},
units::meter_t{translation(1)},
units::meter_t{translation(2)}},
Rotation3d{twist.rx, twist.ry, twist.rz}};
return *this + transform;
}
Twist3d Pose3d::Log(const Pose3d& end) const {
const auto transform = end.RelativeTo(*this);
Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
Matrixd<3, 3> OmegaSq = Omega * Omega;
double thetaSq = rotVec.squaredNorm();
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrixd<3, 3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J⁻¹ = I − 0.5ω + 1/12 ω²
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
} else {
double theta = std::sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
(1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
thetaSq * OmegaSq;
}
// Get dtranslation component
Vectord<3> dtranslation =
Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
transform.Z().value()};
return Twist3d{
units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
}
Pose2d Pose3d::ToPose2d() const {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}

95
photon-lib/src/main/native/cpp/geometry/Quaternion.cpp
View File

@@ -1,95 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <frc/geometry/Quaternion.h>
using namespace frc;
Quaternion::Quaternion(double w, double x, double y, double z)
: m_r{w}, m_v{x, y, z} {}
Quaternion Quaternion::operator*(const Quaternion& other) const {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
const auto& r1 = m_r;
const auto& v1 = m_v;
const auto& r2 = other.m_r;
const auto& v2 = other.m_v;
// v₁ x v₂
Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
v2(0) * v1(2) - v1(0) * v2(2),
v1(0) * v2(1) - v2(0) * v1(1)};
// v = r₁v₂ + r₂v₁ + v₁ x v₂
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
}
bool Quaternion::operator==(const Quaternion& other) const {
return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
}
bool Quaternion::operator!=(const Quaternion& other) const {
return !operator==(other);
}
Quaternion Quaternion::Inverse() const {
return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
}
Quaternion Quaternion::Normalize() const {
double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
if (norm == 0.0) {
return Quaternion{};
} else {
return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
}
}
double Quaternion::W() const { return m_r; }
double Quaternion::X() const { return m_v(0); }
double Quaternion::Y() const { return m_v(1); }
double Quaternion::Z() const { return m_v(2); }
Eigen::Vector3d Quaternion::ToRotationVector() const {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = m_v.norm();
if (norm < 1e-9) {
return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
} else {
if (W() < 0.0) {
return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
} else {
return 2.0 * std::atan2(norm, W()) / norm * m_v;
}
}
}

248
photon-lib/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -1,248 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <cmath>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/QR>
#include <frc/fmt/Eigen.h>
#include <frc/geometry/Rotation3d.h>
#include <units/math.h>
#include <wpi/numbers>
#include "wpimath/MathShared.h"
using namespace frc;
Rotation3d::Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
units::radian_t yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = units::math::cos(roll * 0.5);
double sr = units::math::sin(roll * 0.5);
double cp = units::math::cos(pitch * 0.5);
double sp = units::math::sin(pitch * 0.5);
double cy = units::math::cos(yaw * 0.5);
double sy = units::math::sin(yaw * 0.5);
m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
}
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
const auto& R = rotationMatrix;
// Require that the rotation matrix is special orthogonal. This is true if the
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
std::string msg =
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
wpi::math::MathSharedStore::ReportError(msg);
throw std::domain_error(msg);
}
if (R.determinant() != 1.0) {
std::string msg = fmt::format(
"Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
R);
wpi::math::MathSharedStore::ReportError(msg);
throw std::domain_error(msg);
}
// Turn rotation matrix into a quaternion
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
double trace = R(0, 0) + R(1, 1) + R(2, 2);
double w;
double x;
double y;
double z;
if (trace > 0.0) {
double s = 0.5 / std::sqrt(trace + 1.0);
w = 0.25 / s;
x = (R(2, 1) - R(1, 2)) * s;
y = (R(0, 2) - R(2, 0)) * s;
z = (R(1, 0) - R(0, 1)) * s;
} else {
if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
w = (R(2, 1) - R(1, 2)) / s;
x = 0.25 * s;
y = (R(0, 1) + R(1, 0)) / s;
z = (R(0, 2) + R(2, 0)) / s;
} else if (R(1, 1) > R(2, 2)) {
double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
w = (R(0, 2) - R(2, 0)) / s;
x = (R(0, 1) + R(1, 0)) / s;
y = 0.25 * s;
z = (R(1, 2) + R(2, 1)) / s;
} else {
double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
w = (R(1, 0) - R(0, 1)) / s;
x = (R(0, 2) + R(2, 0)) / s;
y = (R(1, 2) + R(2, 1)) / s;
z = 0.25 * s;
}
}
m_q = Quaternion{w, x, y, z};
}
Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
double dot = initial.dot(final);
double normProduct = initial.norm() * final.norm();
double dotNorm = dot / normProduct;
if (dotNorm > 1.0 - 1E-9) {
// If the dot product is 1, the two vectors point in the same direction so
// there's no rotation. The default initialization of m_q will work.
return;
} else if (dotNorm < -1.0 + 1E-9) {
// If the dot product is -1, the two vectors point in opposite directions so
// a 180 degree rotation is required. Any orthogonal vector can be used for
// it. Q in the QR decomposition is an orthonormal basis, so it contains
// orthogonal unit vectors.
Eigen::Matrix<double, 3, 1> X = initial;
Eigen::HouseholderQR<decltype(X)> qr{X};
Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
// w = std::cos(θ/2) = std::cos(90°) = 0
//
// For x, y, and z, we use the second column of Q because the first is
// parallel instead of orthogonal. The third column would also work.
m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
} else {
// initial x final
Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
final(0) * initial(2) - initial(0) * final(2),
initial(0) * final(1) - final(0) * initial(1)};
// https://stackoverflow.com/a/11741520
m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
}
}
Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
return RotateBy(other);
}
Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
return *this + -other;
}
Rotation3d Rotation3d::operator-() const { return Rotation3d{m_q.Inverse()}; }
Rotation3d Rotation3d::operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
} else {
return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
}
}
bool Rotation3d::operator==(const Rotation3d& other) const {
return m_q == other.m_q;
}
bool Rotation3d::operator!=(const Rotation3d& other) const {
return !operator==(other);
}
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
return Rotation3d{other.m_q * m_q};
}
const Quaternion& Rotation3d::GetQuaternion() const { return m_q; }
units::radian_t Rotation3d::X() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return units::radian_t{
std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
}
units::radian_t Rotation3d::Y() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (std::abs(ratio) >= 1.0) {
return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
} else {
return units::radian_t{std::asin(ratio)};
}
}
units::radian_t Rotation3d::Z() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return units::radian_t{
std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
}
Vectord<3> Rotation3d::Axis() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return {0.0, 0.0, 0.0};
} else {
return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
}
}
units::radian_t Rotation3d::Angle() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
}
Rotation2d Rotation3d::ToRotation2d() const { return Rotation2d{Z()}; }

60
photon-lib/src/main/native/cpp/geometry/Transform3d.cpp
View File

@@ -1,60 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
using namespace frc;
Transform3d::Transform3d(Pose3d initial, Pose3d final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Transform3d Transform3d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
bool Transform3d::operator==(const Transform3d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Transform3d::operator!=(const Transform3d& other) const {
return !operator==(other);
}

88
photon-lib/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -1,88 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <frc/geometry/Translation3d.h>
#include <units/math.h>
using namespace frc;
Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
m_z = rectangular.Z();
}
units::meter_t Translation3d::Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z));
}
units::meter_t Translation3d::Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
units::meter_t{qprime.Z()}};
}
Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
Translation3d Translation3d::operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
Translation3d Translation3d::operator-(const Translation3d& other) const {
return *this + -other;
}
Translation3d Translation3d::operator-() const { return {-m_x, -m_y, -m_z}; }
Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
Translation3d Translation3d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation3d::operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
bool Translation3d::operator!=(const Translation3d& other) const {
return !operator==(other);
}

50
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
View File

@@ -38,13 +38,20 @@ PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
const std::string& cameraName)
: mainTable(instance->GetTable("photonvision")),
rootTable(mainTable->GetSubTable(cameraName)),
rawBytesEntry(rootTable->GetEntry("rawBytes")),
driverModeEntry(rootTable->GetEntry("driverMode")),
inputSaveImgEntry(rootTable->GetEntry("inputSaveImgCmd")),
outputSaveImgEntry(rootTable->GetEntry("outputSaveImgCmd")),
pipelineIndexEntry(rootTable->GetEntry("pipelineIndex")),
ledModeEntry(mainTable->GetEntry("ledMode")),
versionEntry(mainTable->GetEntry("version")),
rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})),
driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
inputSaveImgEntry(
rootTable->GetBooleanTopic("inputSaveImgCmd").Publish()),
outputSaveImgEntry(
rootTable->GetBooleanTopic("outputSaveImgCmd").Publish()),
pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
driverModeSubscriber(
rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
pipelineIndexSubscriber(
rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
path(rootTable->GetPath()) {}
PhotonCamera::PhotonCamera(const std::string& cameraName)
@@ -63,13 +70,10 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
PhotonPipelineResult result;
// Fill the packet with latest data and populate result.
std::shared_ptr<nt::Value> ntvalue = rawBytesEntry.GetValue();
if (!ntvalue) return result;
const auto value = rawBytesEntry.Get();
if (!value.size()) return result;
std::string value{ntvalue->GetRaw()};
std::vector<char> bytes{value.begin(), value.end()};
photonlib::Packet packet{bytes};
photonlib::Packet packet{value};
packet >> result;
@@ -80,31 +84,29 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
}
void PhotonCamera::SetDriverMode(bool driverMode) {
driverModeEntry.SetBoolean(driverMode);
driverModeEntry.Set(driverMode);
}
void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.SetBoolean(true); }
void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.Set(true); }
void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.SetBoolean(true); }
void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.Set(true); }
bool PhotonCamera::GetDriverMode() const {
return driverModeEntry.GetBoolean(false);
}
bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
void PhotonCamera::SetPipelineIndex(int index) {
pipelineIndexEntry.SetDouble(static_cast<double>(index));
pipelineIndexEntry.Set(static_cast<double>(index));
}
int PhotonCamera::GetPipelineIndex() const {
return static_cast<int>(pipelineIndexEntry.GetDouble(0));
return static_cast<int>(pipelineIndexSubscriber.Get());
}
LEDMode PhotonCamera::GetLEDMode() const {
return static_cast<LEDMode>(static_cast<int>(ledModeEntry.GetDouble(-1.0)));
return static_cast<LEDMode>(static_cast<int>(ledModeSubscriber.Get()));
}
void PhotonCamera::SetLEDMode(LEDMode mode) {
ledModeEntry.SetDouble(static_cast<double>(static_cast<int>(mode)));
ledModeEntry.Set(static_cast<double>(static_cast<int>(mode)));
}
void PhotonCamera::VerifyVersion() {
@@ -115,7 +117,7 @@ void PhotonCamera::VerifyVersion() {
return;
this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();
const std::string& versionString = versionEntry.GetString("");
const std::string& versionString = versionEntry.Get("");
if (versionString.empty()) {
std::string path_ = path;
FRC_ReportError(

2
photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
View File

@@ -26,7 +26,7 @@
namespace photonlib {
PhotonPipelineResult::PhotonPipelineResult(
units::second_t latency, wpi::span<const PhotonTrackedTarget> targets)
units::second_t latency, std::span<const PhotonTrackedTarget> targets)
: latency(latency),
targets(targets.data(), targets.data() + targets.size()) {}

200
photon-lib/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -1,200 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Pose2d.h>
#include <wpi/SymbolExports.h>
#include "Transform3d.h"
#include "Translation3d.h"
#include "Twist3d.h"
namespace frc {
/**
* Represents a 3D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose3d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
*/
constexpr Pose3d() = default;
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs a pose with x, y, and z translations instead of a separate
* Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d operator+(const Transform3d& other) const;
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform3d operator-(const Pose3d& other) const;
/**
* Checks equality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Pose3d& other) const;
/**
* Checks inequality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Pose3d& other) const;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d TransformBy(const Transform3d& other) const;
/**
* Returns the other pose relative to the current pose.
*
* This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
*
* @return The current pose relative to the new origin pose.
*/
Pose3d RelativeTo(const Pose3d& other) const;
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
* 0.5_deg}}.
*
* @return The new pose of the robot.
*/
Pose3d Exp(const Twist3d& twist) const;
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
Twist3d Log(const Pose3d& end) const;
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/
Pose2d ToPose2d() const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

114
photon-lib/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -1,114 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/EigenCore.h>
#include <wpi/SymbolExports.h>
namespace frc {
class WPILIB_DLLEXPORT Quaternion {
public:
/**
* Constructs a quaternion with a default angle of 0 degrees.
*/
Quaternion() = default;
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
Quaternion(double w, double x, double y, double z);
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
*/
Quaternion operator*(const Quaternion& other) const;
/**
* Checks equality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Quaternion& other) const;
/**
* Checks inequality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Quaternion& other) const;
/**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
/**
* Normalizes the quaternion.
*/
Quaternion Normalize() const;
/**
* Returns W component of the quaternion.
*/
double W() const;
/**
* Returns X component of the quaternion.
*/
double X() const;
/**
* Returns Y component of the quaternion.
*/
double Y() const;
/**
* Returns Z component of the quaternion.
*/
double Z() const;
/**
* Returns the rotation vector representation of this quaternion.
*
* This is also the log operator of SO(3).
*/
Eigen::Vector3d ToRotationVector() const;
private:
double m_r = 1.0;
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
};
} // namespace frc

193
photon-lib/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -1,193 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/EigenCore.h>
#include <frc/geometry/Rotation2d.h>
#include <units/angle.h>
#include <wpi/SymbolExports.h>
#include "Quaternion.h"
namespace frc {
/**
* A rotation in a 3D coordinate frame represented by a quaternion.
*/
class WPILIB_DLLEXPORT Rotation3d {
public:
/**
* Constructs a Rotation3d with a default angle of 0 degrees.
*/
Rotation3d() = default;
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
explicit Rotation3d(const Quaternion& q);
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* Extrinsic rotations occur in that order around the axes in the fixed global
* frame rather than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll).
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis
* doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Vectord<3>& axis, units::radian_t angle);
/**
* Constructs a Rotation3d from a rotation matrix.
*
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
/**
* Constructs a Rotation3d that rotates the initial vector onto the final
* vector.
*
* This is useful for turning a 3D vector (final) into an orientation relative
* to a coordinate system vector (initial).
*
* @param initial The initial vector.
* @param final The final vector.
*/
Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
/**
* Adds two rotations together.
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
Rotation3d operator+(const Rotation3d& other) const;
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
*/
Rotation3d operator-(const Rotation3d& other) const;
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
Rotation3d operator-() const;
/**
* Multiplies the current rotation by a scalar.
* @param scalar The scalar.
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator*(double scalar) const;
/**
* Checks equality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation3d& other) const;
/**
* Checks inequality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation3d& other) const;
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
*
* @return The new rotated Rotation3d.
*/
Rotation3d RotateBy(const Rotation3d& other) const;
/**
* Returns the quaternion representation of the Rotation3d.
*/
const Quaternion& GetQuaternion() const;
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
units::radian_t X() const;
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
units::radian_t Y() const;
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
units::radian_t Z() const;
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Vectord<3> Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
units::radian_t Angle() const;
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.
*/
Rotation2d ToRotation2d() const;
private:
Quaternion m_q;
};
} // namespace frc

141
photon-lib/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -1,141 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <wpi/SymbolExports.h>
#include "Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
/**
* Represents a transformation for a Pose3d.
*/
class WPILIB_DLLEXPORT Transform3d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform3d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform3d& other) const;
/**
* Checks inequality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform3d& other) const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

205
photon-lib/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -1,205 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
#include <wpi/SymbolExports.h>
#include "Rotation3d.h"
namespace frc {
/**
* Represents a translation in 3D space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes. When the
* robot is at the origin facing in the positive X direction, forward is
* positive X, left is positive Y, and up is positive Z.
*/
class WPILIB_DLLEXPORT Translation3d {
public:
/**
* Constructs a Translation3d with X, Y, and Z components equal to zero.
*/
constexpr Translation3d() = default;
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
/**
* Constructs a Translation3d with the provided distance and angle. This is
* essentially converting from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Calculates the distance between two translations in 3D space.
*
* The distance between translations is defined as
* √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
/**
* Returns the X component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t Z() const { return m_z; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 3D space.
*
* For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
* around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const;
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
Translation2d ToTranslation2d() const;
/**
* Returns the sum of two translations in 3D space.
*
* For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
* Translation3d{3.0, 8.0, 11.0}.
*
* @param other The translation to add.
*
* @return The sum of the translations.
*/
Translation3d operator+(const Translation3d& other) const;
/**
* Returns the difference between two translations.
*
* For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
* Translation3d{4.0, 2.0, 0.0}.
*
* @param other The translation to subtract.
*
* @return The difference between the two translations.
*/
Translation3d operator-(const Translation3d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
* negating all components of the translation.
*
* @return The inverse of the current translation.
*/
Translation3d operator-() const;
/**
* Returns the translation multiplied by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
* 9.0}.
*
* @param scalar The scalar to multiply by.
*
* @return The scaled translation.
*/
Translation3d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
* 2.25}.
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
Translation3d operator/(double scalar) const;
/**
* Checks equality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
/**
* Checks inequality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Translation3d& other) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
units::meter_t m_z = 0_m;
};
} // namespace frc

106
photon-lib/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -1,106 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Rotation3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
#include <wpi/SymbolExports.h>
namespace frc {
/**
* A change in distance along a 3D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose3ds from a Twist3d and
* vise versa.
*
* A Twist can be used to represent a difference between two poses.
*/
struct WPILIB_DLLEXPORT Twist3d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
/**
* Linear "dz" component
*/
units::meter_t dz = 0_m;
/**
* Rotation vector x component.
*/
units::radian_t rx = 0_rad;
/**
* Rotation vector y component.
*/
units::radian_t ry = 0_rad;
/**
* Rotation vector z component.
*/
units::radian_t rz = 0_rad;
/**
* Checks equality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist3d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dz - other.dz) < 1E-9_m &&
units::math::abs(rx - other.rx) < 1E-9_rad &&
units::math::abs(ry - other.ry) < 1E-9_rad &&
units::math::abs(rz - other.rz) < 1E-9_rad;
}
/**
* Checks inequality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist3d& other) const { return !operator==(other); }
/**
* Scale this by a given factor.
*
* @param factor The factor by which to scale.
* @return The scaled Twist3d.
*/
Twist3d operator*(double factor) const {
return Twist3d{dx * factor, dy * factor, dz * factor,
rx * factor, ry * factor, rz * factor};
}
};
} // namespace frc

8
photon-lib/src/main/native/include/photonlib/Packet.h
View File

@@ -46,7 +46,7 @@ class Packet {
* Constructs a packet with the given data.
* @param data The packet data.
*/
explicit Packet(std::vector<char> data) : packetData(data) {}
explicit Packet(std::vector<uint8_t> data) : packetData(data) {}
/**
* Clears the packet and resets the read and write positions.
@@ -61,7 +61,7 @@ class Packet {
* Returns the packet data.
* @return The packet data.
*/
const std::vector<char>& GetData() { return packetData; }
const std::vector<uint8_t>& GetData() { return packetData; }
/**
* Returns the number of bytes in the data.
@@ -105,7 +105,7 @@ class Packet {
if constexpr (wpi::support::endian::system_endianness() ==
wpi::support::endianness::little) {
// Reverse to little endian for host.
char& raw = reinterpret_cast<char&>(value);
uint8_t& raw = reinterpret_cast<uint8_t&>(value);
std::reverse(&raw, &raw + sizeof(T));
}
}
@@ -121,7 +121,7 @@ class Packet {
private:
// Data stored in the packet
std::vector<char> packetData;
std::vector<uint8_t> packetData;
size_t readPos = 0;
size_t writePos = 0;

24
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
View File

@@ -27,9 +27,13 @@
#include <memory>
#include <string>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/RawTopic.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/deprecated.h>
@@ -146,13 +150,17 @@ class PhotonCamera {
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
std::shared_ptr<nt::NetworkTable> rootTable;
nt::NetworkTableEntry rawBytesEntry;
nt::NetworkTableEntry driverModeEntry;
nt::NetworkTableEntry inputSaveImgEntry;
nt::NetworkTableEntry outputSaveImgEntry;
nt::NetworkTableEntry pipelineIndexEntry;
nt::NetworkTableEntry ledModeEntry;
nt::NetworkTableEntry versionEntry;
nt::RawSubscriber rawBytesEntry;
nt::BooleanPublisher driverModeEntry;
nt::BooleanPublisher inputSaveImgEntry;
nt::BooleanPublisher outputSaveImgEntry;
nt::IntegerPublisher pipelineIndexEntry;
nt::IntegerPublisher ledModeEntry;
nt::StringSubscriber versionEntry;
nt::BooleanSubscriber driverModeSubscriber;
nt::IntegerSubscriber pipelineIndexSubscriber;
nt::IntegerSubscriber ledModeSubscriber;
std::string path;

6
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
View File

@@ -24,12 +24,12 @@
#pragma once
#include <span>
#include <string>
#include <frc/Errors.h>
#include <units/time.h>
#include <wpi/SmallVector.h>
#include <wpi/span.h>
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
@@ -51,7 +51,7 @@ class PhotonPipelineResult {
* @param targets The list of targets identified by the pipeline.
*/
PhotonPipelineResult(units::second_t latency,
wpi::span<const PhotonTrackedTarget> targets);
std::span<const PhotonTrackedTarget> targets);
/**
* Returns the best target in this pipeline result. If there are no targets,
@@ -105,7 +105,7 @@ class PhotonPipelineResult {
* Returns a reference to the vector of targets.
* @return A reference to the vector of targets.
*/
const wpi::span<const PhotonTrackedTarget> GetTargets() const {
const std::span<const PhotonTrackedTarget> GetTargets() const {
return targets;
}

6
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -83,6 +83,12 @@ class PhotonTrackedTarget {
*/
double GetSkew() const { return skew; }
/**
* Get the Fiducial ID of the target currently being tracked,
* or -1 if not set.
*/
double GetFiducialId() const { return fiducialId; }
/**
* Returns the corners of the minimum area rectangle bounding this target.
*/

19
photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h
View File

@@ -47,6 +47,7 @@ class SimPhotonCamera : public PhotonCamera {
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
versionEntry = instance->GetTable("photonvision")->GetEntry("version");
// versionEntry.SetString(PhotonVersion.versionString);
}
@@ -86,10 +87,9 @@ class SimPhotonCamera : public PhotonCamera {
PhotonPipelineResult newResult{latency, targetList};
Packet packet{};
packet << newResult;
rawBytesEntry.SetRaw(
std::string_view{packet.GetData().data(), packet.GetDataSize()});
std::string rawBytesGet = rawBytesEntry.GetRaw("ohono");
rawBytesPublisher.Set(
std::span{packet.GetData().data(), packet.GetDataSize()});
bool hasTargets = newResult.HasTargets();
hasTargetEntry.SetBoolean(hasTargets);
@@ -97,7 +97,8 @@ class SimPhotonCamera : public PhotonCamera {
targetPitchEntry.SetDouble(0.0);
targetYawEntry.SetDouble(0.0);
targetAreaEntry.SetDouble(0.0);
targetPoseEntry.SetDoubleArray({0.0, 0.0, 0.0});
targetPoseEntry.SetDoubleArray(
std::vector<double>{0.0, 0.0, 0.0, 0, 0, 0, 0});
targetSkewEntry.SetDouble(0.0);
} else {
PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
@@ -107,9 +108,12 @@ class SimPhotonCamera : public PhotonCamera {
targetSkewEntry.SetDouble(bestTarget.GetSkew());
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
targetPoseEntry.SetDoubleArray(
{transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()});
targetPoseEntry.SetDoubleArray(std::vector<double>{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Z().to<double>(), transform.Rotation().GetQuaternion().W(),
transform.Rotation().GetQuaternion().X(),
transform.Rotation().GetQuaternion().Y(),
transform.Rotation().GetQuaternion().Z()});
}
}
@@ -122,5 +126,6 @@ class SimPhotonCamera : public PhotonCamera {
nt::NetworkTableEntry targetSkewEntry;
nt::NetworkTableEntry targetPoseEntry;
nt::NetworkTableEntry versionEntry;
nt::RawPublisher rawBytesPublisher;
};
} // namespace photonlib

18
photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java
View File

@@ -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 edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -36,7 +37,11 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.List;
import java.util.stream.Stream;
import org.junit.jupiter.api.AfterAll;
@@ -65,6 +70,19 @@ class SimVisionSystemTest {
@BeforeAll
public static void setUp() {
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
try {
CombinedRuntimeLoader.loadLibraries(
SimVisionSystem.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
// NT live for debug purposes
NetworkTableInstance.getDefault().startServer();

42
photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
View File

@@ -119,27 +119,28 @@ TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) {
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
}
TEST_F(SimVisionSystemTest, TestNotVisibleVertTwo) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 2_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
frc::Transform3d robotToCamera{
frc::Translation3d{0_m, 0_m, 1_m},
frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad,
0_deg}};
photonlib::SimVisionSystem sys{
"Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
// TEST_F(SimVisionSystemTest, TestNotVisibleVertTwo) {
// const frc::Pose3d targetPose{
// {15.98_m, 0_m, 2_m},
// frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL *
// 1_rad}};
// frc::Transform3d robotToCamera{
// frc::Translation3d{0_m, 0_m, 1_m},
// frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad,
// 0_deg}};
// photonlib::SimVisionSystem sys{
// "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0};
// sys.AddSimVisionTarget(
// photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
// frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}};
// sys.ProcessFrame(robotPose);
// ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
}
// robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m},
// frc::Rotation2d{5_deg}}; sys.ProcessFrame(robotPose);
// ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// }
TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) {
const frc::Pose3d targetPose{
@@ -355,8 +356,7 @@ TEST_F(SimVisionSystemTest, TestMultipleTargets) {
ASSERT_TRUE(results.HasTargets());
wpi::span<const photonlib::PhotonTrackedTarget> targetList =
results.GetTargets();
auto targetList = results.GetTargets();
ASSERT_EQ(targetList.size(), size_t(11));
}

46
photon-server/build.gradle
View File

@@ -1,3 +1,7 @@
plugins {
id 'edu.wpi.first.WpilibTools' version '1.0.0'
}
apply plugin: "application"
apply plugin: "com.github.johnrengelman.shadow"
apply plugin: "org.hidetake.ssh"
@@ -11,12 +15,34 @@ version versionString + (project.hasProperty('pionly') ? "-raspi" : "")
apply from: "${rootDir}/shared/common.gradle"
def nativeConfigName = 'wpilibTestNative'
def nativeConfig = configurations.create(nativeConfigName)
def nativeTasks = wpilibTools.createExtractionTasks { configurationName = nativeConfigName }
nativeTasks.addToSourceSetResources(sourceSets.main)
nativeConfig.dependencies.add wpilibTools.deps.cscore()
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
dependencies {
implementation project(':photon-core')
implementation project(':photon-targeting')
implementation "io.javalin:javalin:4.2.0"
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("wpinet")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation "org.msgpack:msgpack-core:0.9.0"
implementation "org.msgpack:jackson-dataformat-msgpack:0.9.0"
@@ -26,7 +52,10 @@ dependencies {
shadowJar {
configurations = [project.configurations.runtimeClasspath]
String name = "photonvision-${project.version}"
archiveFileName.set("${name}.jar")
archiveClassifier.set(wpilibTools.platformMapper.currentPlatform.platformName)
archiveBaseName = "photonvision"
archiveVersion = project.version
// archiveFileName.set("${name}.jar")
}
task runNpmOnClient(type: Exec) {
@@ -124,11 +153,22 @@ run {
environment "PATH_PREFIX", "../"
}
// task overrideToPi {
// doLast {
// project.setProperty('ArchOverride', 'linuxarm32')
// }
// }
task deploy {
//dependsOn overrideToPi
dependsOn assemble
dependsOn findDeployTarget
doLast {
doLast {
println 'Starting deployment to ' + findDeployTarget.rmt.host
println 'targetArch = ' + wpilibTools.platformMapper.currentPlatform.platformName
ssh.run{
session(findDeployTarget.rmt) {
//Stop photonvision before manipulating its files
@@ -136,7 +176,7 @@ task deploy {
// gerth2 - I was having issues with the .jar being in use still - waiting a tiny bit here seems to get rid of it on a pi4
execute 'sleep 3'
// Copy into a folder owned by PI. Mostly because, as far as I can tell, the put command doesn't support sudo.
put from: "${projectDir}/build/libs/photonvision-${project.version}.jar", into: "/tmp/photonvision.jar"
put from: "${projectDir}/build/libs/photonvision-${project.version}-${wpilibTools.platformMapper.currentPlatform.platformName}.jar", into: "/tmp/photonvision.jar"
//belt-and-suspenders. Make sure the old jar is gone first.
execute 'sudo rm -f /opt/photonvision/photonvision.jar'
//Copy in the new .jar and make sure it's executable

BIN
photon-server/lib/apriltag.dll
View File

Binary file not shown.

30
photon-server/src/main/java/org/photonvision/Main.java
View File

@@ -17,7 +17,6 @@
package org.photonvision;
import edu.wpi.first.cscore.CameraServerCvJNI;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
@@ -37,9 +36,7 @@ import org.photonvision.common.logging.Logger;
import org.photonvision.common.networking.NetworkManager;
import org.photonvision.common.util.TestUtils;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.raspi.PicamJNI;
import org.photonvision.server.Server;
import org.photonvision.vision.apriltag.AprilTagJNI;
import org.photonvision.vision.camera.FileVisionSource;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.ContourGroupingMode;
@@ -289,21 +286,17 @@ public class Main {
public static void main(String[] args) {
try {
CameraServerCvJNI.forceLoad();
TestUtils.loadLibraries();
logger.info("Native libraries loaded.");
} catch (Exception e) {
logger.error("Failed to load native libraries!", e);
}
try {
AprilTagJNI.forceLoad();
} catch (IOException e) {
logger.error("Failed to load native libraries!", e);
}
try {
PicamJNI.forceLoad();
} catch (IOException e) {
logger.error("Failed to load native libraries!", e);
}
// try {
// PicamJNI.forceLoad();
// } catch (IOException e) {
// logger.error("Failed to load Picam JNI!", e);
// }
try {
if (!handleArgs(args)) {
@@ -331,15 +324,6 @@ public class Main {
+ Platform.currentPlatform.toString()
+ (Platform.isRaspberryPi() ? (" (Pi " + Platform.currentPiVersion.name() + ")") : ""));
try {
CameraServerCvJNI.forceLoad();
PicamJNI.forceLoad();
// TestUtils.loadLibraries();
logger.info("Native libraries loaded.");
} catch (Exception e) {
logger.error("Failed to load native libraries!", e);
}
ConfigManager.getInstance().load(); // init config manager
ConfigManager.getInstance().requestSave();

BIN
photon-server/src/main/resources/nativelibraries/apriltag/aarch64/libapriltag.so
View File

Binary file not shown.

BIN
photon-server/src/main/resources/nativelibraries/apriltag/athena/libapriltag.so
View File

Binary file not shown.

BIN
photon-server/src/main/resources/nativelibraries/apriltag/linux64/libapriltag.so
View File

Binary file not shown.

BIN
photon-server/src/main/resources/nativelibraries/apriltag/mac/libapriltag.dylib
View File

Binary file not shown.

BIN
photon-server/src/main/resources/nativelibraries/apriltag/raspbian/libapriltag.so
View File

Binary file not shown.

BIN
photon-server/src/main/resources/nativelibraries/apriltag/win64/apriltag.dll
View File

Binary file not shown.

2
photon-targeting/build.gradle
View File

@@ -6,7 +6,7 @@ sourceCompatibility = 11
targetCompatibility = 11
dependencies {
implementation "edu.wpi.first.wpimath:wpimath-java:$wpilibVersion"
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation "org.ejml:ejml-simple:0.41"
}

108
photon-targeting/src/main/java/edu/wpi/first/math/Vector.java
View File

@@ -1,108 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math;
import edu.wpi.first.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
* <p>This class is intended to be used alongside the state space library.
*
* @param <R> The number of rows in this matrix.
*/
public class Vector<R extends Num> extends Matrix<R, N1> {
/**
* Constructs an empty zero vector of the given dimensions.
*
* @param rows The number of rows of the vector.
*/
public Vector(Nat<R> rows) {
super(rows, Nat.N1());
}
/**
* Constructs a new {@link Vector} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Vector}.
*
* <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
* absolutely necessary due to the desired function not being accessible through the {@link
* Vector} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this vector.
*/
public Vector(SimpleMatrix storage) {
super(storage);
}
/**
* Constructs a new vector with the storage of the supplied matrix.
*
* @param other The {@link Vector} to copy the storage of.
*/
public Vector(Matrix<R, N1> other) {
super(other);
}
@Override
public Vector<R> times(double value) {
return new Vector<>(this.m_storage.scale(value));
}
@Override
public Vector<R> div(int value) {
return new Vector<>(this.m_storage.divide(value));
}
@Override
public Vector<R> div(double value) {
return new Vector<>(this.m_storage.divide(value));
}
/**
* Returns the dot product of this vector with another.
*
* @param other The other vector.
* @return The dot product.
*/
public double dot(Vector<R> other) {
double dot = 0.0;
for (int i = 0; i < getNumRows(); ++i) {
dot += get(i, 0) * other.get(i, 0);
}
return dot;
}
/**
* Returns the norm of this vector.
*
* @return The norm.
*/
public double norm() {
double squaredNorm = 0.0;
for (int i = 0; i < getNumRows(); ++i) {
squaredNorm += get(i, 0) * get(i, 0);
}
return Math.sqrt(squaredNorm);
}
}

99
photon-targeting/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
View File

@@ -1,99 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
/** A class representing a coordinate system axis within the NWU coordinate system. */
public class CoordinateAxis {
final Vector<N3> m_axis;
/**
* Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
*
* @param x The x component.
* @param y The y component.
* @param z The z component.
*/
public CoordinateAxis(double x, double y, double z) {
double norm = Math.sqrt(x * x + y * y + z * z);
m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
}
/**
* Returns a coordinate axis corresponding to +X in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +X in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis N() {
return new CoordinateAxis(1.0, 0.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to -X in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -X in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis S() {
return new CoordinateAxis(-1.0, 0.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -Y in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis E() {
return new CoordinateAxis(0.0, -1.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +Y in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis W() {
return new CoordinateAxis(0.0, 1.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +Z in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis U() {
return new CoordinateAxis(0.0, 0.0, 1.0);
}
/**
* Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -Z in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis D() {
return new CoordinateAxis(0.0, 0.0, -1.0);
}
}

134
photon-targeting/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
View File

@@ -1,134 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
/** A helper class that converts Pose3d objects between different standard coordinate frames. */
public class CoordinateSystem {
// Rotation from this coordinate system to NWU coordinate system
private final Rotation3d m_rotation;
/**
* Constructs a coordinate system with the given cardinal directions for each axis.
*
* @param positiveX The cardinal direction of the positive x-axis.
* @param positiveY The cardinal direction of the positive y-axis.
* @param positiveZ The cardinal direction of the positive z-axis.
* @throws IllegalArgumentException if the coordinate system isn't special orthogonal
*/
public CoordinateSystem(
CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
// Construct a change of basis matrix from the source coordinate system to the
// NWU coordinate system. Each column vector in the change of basis matrix is
// one of the old basis vectors mapped to its representation in the new basis.
@SuppressWarnings("LocalVariableName")
var R = new Matrix<>(Nat.N3(), Nat.N3());
R.assignBlock(0, 0, positiveX.m_axis);
R.assignBlock(0, 1, positiveY.m_axis);
R.assignBlock(0, 2, positiveZ.m_axis);
// Require that the change of basis matrix is special orthogonal. This is true
// if the axes used are orthogonal and normalized. The Axis class already
// normalizes itself, so we just need to check for orthogonality.
if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
}
// Turn change of basis matrix into a quaternion since it's a pure rotation
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
double w;
double x;
double y;
double z;
if (trace > 0.0) {
double s = 0.5 / Math.sqrt(trace + 1.0);
w = 0.25 / s;
x = (R.get(2, 1) - R.get(1, 2)) * s;
y = (R.get(0, 2) - R.get(2, 0)) * s;
z = (R.get(1, 0) - R.get(0, 1)) * s;
} else {
if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
w = (R.get(2, 1) - R.get(1, 2)) / s;
x = 0.25 * s;
y = (R.get(0, 1) + R.get(1, 0)) / s;
z = (R.get(0, 2) + R.get(2, 0)) / s;
} else if (R.get(1, 1) > R.get(2, 2)) {
double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
w = (R.get(0, 2) - R.get(2, 0)) / s;
x = (R.get(0, 1) + R.get(1, 0)) / s;
y = 0.25 * s;
z = (R.get(1, 2) + R.get(2, 1)) / s;
} else {
double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
w = (R.get(1, 0) - R.get(0, 1)) / s;
x = (R.get(0, 2) + R.get(2, 0)) / s;
y = (R.get(1, 2) + R.get(2, 1)) / s;
z = 0.25 * s;
}
}
m_rotation = new Rotation3d(new Quaternion(w, x, y, z));
}
/**
* Returns an instance of the NWU coordinate system.
*
* @return An instance of the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem NWU() {
return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
}
/**
* Returns an instance of the EDN coordinate system.
*
* @return An instance of the EDN coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem EDN() {
return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
}
/**
* Returns an instance of the NED coordinate system.
*
* @return An instance of the NED coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem NED() {
return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
}
/**
* Converts the given pose from one coordinate system to another.
*
* @param pose The pose to convert.
* @param from The coordinate system the pose starts in.
* @param to The coordinate system to which to convert.
* @return The given pose in the desired coordinate system.
*/
public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation)));
}
}

387
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -1,387 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose3d implements Interpolatable<Pose3d> {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
@JsonCreator
public Pose3d(
@JsonProperty(required = true, value = "translation") Translation3d translation,
@JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a pose with x, y, and z translations instead of a separate Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(double x, double y, double z, Rotation3d rotation) {
m_translation = new Translation3d(x, y, z);
m_rotation = rotation;
}
public Pose3d(Transform3d transform) {
this(transform.getTranslation(), transform.getRotation());
}
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d plus(Transform3d other) {
return transformBy(other);
}
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform3d minus(Pose3d other) {
final var pose = this.relativeTo(other);
return new Transform3d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
@JsonProperty
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
@JsonProperty
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d transformBy(Transform3d other) {
return new Pose3d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
*
* @param other The pose that is the origin of the new coordinate frame that the current pose will
* be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose3d relativeTo(Pose3d other) {
var transform = new Transform3d(other, this);
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
* Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose3d exp(Twist3d twist) {
final var Omega = hatSO3(VecBuilder.fill(twist.droll, twist.dpitch, twist.dyaw));
final var OmegaSq = Omega.times(Omega);
double thetaSq =
twist.droll * twist.droll + twist.dpitch * twist.dpitch + twist.dyaw * twist.dyaw;
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> J;
if (thetaSq < 1E-9 * 1E-9) {
// J = I + 0.5ω
J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
} else {
double theta = Math.sqrt(thetaSq);
// J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
J =
Matrix.eye(Nat.N3())
.plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
.plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
}
// Get translation component
final var translation =
J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
final var transform =
new Transform3d(
new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
new Rotation3d(twist.droll, twist.dpitch, twist.dyaw));
return this.plus(transform);
}
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output of a.Log(b), then
* a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
@SuppressWarnings("LocalVariableName")
public Twist3d log(Pose3d end) {
final var transform = end.relativeTo(this);
var rotVec = logSO3(transform.getRotation());
final var Omega = hatSO3(rotVec);
final var OmegaSq = Omega.times(Omega);
double thetaSq =
rotVec.get(0, 0) * rotVec.get(0, 0)
+ rotVec.get(1, 0) * rotVec.get(1, 0)
+ rotVec.get(2, 0) * rotVec.get(2, 0);
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J^-1 = I − 0.5ω + 1/12 ω²
Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
} else {
double theta = Math.sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J^-1 = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
Jinv =
Matrix.eye(Nat.N3())
.minus(Omega.times(0.5))
.plus(
OmegaSq.times(
(1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
}
// Get dtranslation component
final var dtranslation =
Jinv.times(
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(transform.getX(), transform.getY(), transform.getZ()));
return new Twist3d(
dtranslation.get(0, 0),
dtranslation.get(1, 0),
dtranslation.get(2, 0),
rotVec.get(0, 0),
rotVec.get(1, 0),
rotVec.get(2, 0));
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*
* @return A Pose2d representing this Pose3d projected into the X-Y plane.
*/
public Pose2d toPose2d() {
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
/**
* Checks equality between this Pose3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose3d) {
return ((Pose3d) obj).m_translation.equals(m_translation)
&& ((Pose3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
@SuppressWarnings("ParameterName")
public Pose3d interpolate(Pose3d endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var scaledTwist =
new Twist3d(
twist.dx * t,
twist.dy * t,
twist.dz * t,
twist.droll * t,
twist.dpitch * t,
twist.dyaw * t);
return this.exp(scaledTwist);
}
}
/**
* Applies the log operator to a rotation.
*
* <p>It takes a quaternion and returns a rotation vector.
*
* @param rotation The rotation.
* @return The rotation vector.
*/
private Vector<N3> logSO3(Rotation3d rotation) {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
final var q = rotation.getQuaternion();
double norm = Math.sqrt(q.getX() * q.getX() + q.getY() * q.getY() + q.getZ() * q.getZ());
// The quaternion in a Rotation3d is already guaranteed to have a nonzero
// vector component and be normalized, so no divide-by-zero checks are
// performed.
if (q.getW() < 0.0) {
return VecBuilder.fill(q.getX(), q.getY(), q.getZ())
.times(2.0 * Math.atan2(-norm, -q.getW()) / norm);
} else {
return VecBuilder.fill(q.getX(), q.getY(), q.getZ())
.times(2.0 * Math.atan2(norm, q.getW()) / norm);
}
}
/**
* Applies the hat operator to a rotation vector.
*
* <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
* algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
private Matrix<N3, N3> hatSO3(Vector<N3> rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return new MatBuilder<>(Nat.N3(), Nat.N3())
.fill(
0.0,
-rotation.get(2, 0),
rotation.get(1, 0),
rotation.get(2, 0),
0.0,
-rotation.get(0, 0),
-rotation.get(1, 0),
rotation.get(0, 0),
0.0);
}
}

199
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
View File

@@ -1,199 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Quaternion {
private final double m_r;
private final Vector<N3> m_v;
/** Constructs a quaternion with a default angle of 0 degrees. */
public Quaternion() {
m_r = 1.0;
m_v = VecBuilder.fill(0.0, 0.0, 0.0);
}
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
@JsonCreator
public Quaternion(
@JsonProperty(required = true, value = "W") double w,
@JsonProperty(required = true, value = "X") double x,
@JsonProperty(required = true, value = "Y") double y,
@JsonProperty(required = true, value = "Z") double z) {
m_r = w;
m_v = VecBuilder.fill(x, y, z);
}
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
* @return The quaternion product.
*/
public Quaternion times(Quaternion other) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
// v₁ x v₂
var cross =
VecBuilder.fill(
v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
// v = r₁v₂ + r₂v₁ + v₁ x v₂
final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
@Override
public String toString() {
return String.format(
"Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
}
/**
* Checks equality between this Quaternion and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Quaternion) {
var other = (Quaternion) obj;
return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_r, m_v);
}
/**
* Returns the inverse of the quaternion.
*
* @return The inverse quaternion.
*/
public Quaternion inverse() {
return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
}
/**
* Normalizes the quaternion.
*
* @return The normalized quaternion.
*/
public Quaternion normalize() {
double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
if (norm == 0.0) {
return new Quaternion();
} else {
return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
}
}
/**
* Returns W component of the quaternion.
*
* @return W component of the quaternion.
*/
@JsonProperty(value = "W")
public double getW() {
return m_r;
}
/**
* Returns X component of the quaternion.
*
* @return X component of the quaternion.
*/
@JsonProperty(value = "X")
public double getX() {
return m_v.get(0, 0);
}
/**
* Returns Y component of the quaternion.
*
* @return Y component of the quaternion.
*/
@JsonProperty(value = "Y")
public double getY() {
return m_v.get(1, 0);
}
/**
* Returns Z component of the quaternion.
*
* @return Z component of the quaternion.
*/
@JsonProperty(value = "Z")
public double getZ() {
return m_v.get(2, 0);
}
/**
* Returns the rotation vector representation of this quaternion.
*
* <p>This is also the log operator of SO(3).
*
* @return The rotation vector representation of this quaternion.
*/
public Vector<N3> toRotationVector() {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = m_v.norm();
if (norm < 1e-9) {
return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
} else {
if (getW() < 0.0) {
return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
} else {
return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
}
}
}
}

291
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -1,291 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** A rotation in a 3D coordinate frame represented by a quaternion. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation3d implements Interpolatable<Rotation3d> {
private Quaternion m_q = new Quaternion();
/** Constructs a Rotation3d with a default angle of 0 degrees. */
public Rotation3d() {}
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
@JsonCreator
public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
m_q = q.normalize();
}
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
* than the body frame.
*
* @param roll The roll angle in radians.
* @param pitch The pitch angle in radians.
* @param yaw The yaw angle in radians.
*/
public Rotation3d(double roll, double pitch, double yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = Math.cos(roll * 0.5);
double sr = Math.sin(roll * 0.5);
double cp = Math.cos(pitch * 0.5);
double sp = Math.sin(pitch * 0.5);
double cy = Math.cos(yaw * 0.5);
double sy = Math.sin(yaw * 0.5);
m_q =
new Quaternion(
cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy);
}
/**
* Constructs a Rotation3d with the given axis and angle. The axis doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angleRadians The rotation around the axis in radians.
*/
public Rotation3d(Vector<N3> axis, double angleRadians) {
double norm = axis.normF();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
/**
* Constructs a quaternion from a 3x3, row-major direction cosine matrix
* https://intra.ece.ucr.edu/~farrell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf
*
* @param dcm A 3x3 direction cosine matrix
*/
public Rotation3d(Matrix<N3, N3> dcm) {
double b1 = 0.5 * Math.sqrt(1 + dcm.get(0, 0) + dcm.get(1, 1) + dcm.get(2, 2));
double b2 = (dcm.get(2, 1) - dcm.get(1, 2)) / (4 * b1);
double b3 = (dcm.get(0, 2) - dcm.get(2, 0)) / (4 * b1);
double b4 = (dcm.get(1, 0) - dcm.get(0, 1)) / (4 * b1);
m_q = new Quaternion(b1, b2, b3, b4).normalize();
}
/**
* Adds two rotations together.
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation3d plus(Rotation3d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation3d minus(Rotation3d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
public Rotation3d unaryMinus() {
return new Rotation3d(m_q.inverse());
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation3d.
*/
public Rotation3d times(double scalar) {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.getW() >= 0.0) {
return new Rotation3d(
VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
2.0 * scalar * Math.acos(m_q.getW()));
} else {
return new Rotation3d(
VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
2.0 * scalar * Math.acos(-m_q.getW()));
}
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
*
* @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
*/
public Rotation2d toRotation2d() {
return new Rotation2d(getZ());
}
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation3d.
*/
public Rotation3d rotateBy(Rotation3d other) {
return new Rotation3d(other.m_q.times(m_q));
}
/**
* Returns the quaternion representation of the Rotation3d.
*
* @return The quaternion representation of the Rotation3d.
*/
@JsonProperty(value = "quaternion")
public Quaternion getQuaternion() {
return m_q;
}
/**
* Returns the counterclockwise rotation angle around the X axis (roll) in radians.
*
* @return The counterclockwise rotation angle around the X axis (roll) in radians.
*/
public double getX() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
}
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
*
* @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
*/
public double getY() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (Math.abs(ratio) >= 1.0) {
return Math.copySign(Math.PI / 2.0, ratio);
} else {
return Math.asin(ratio);
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
*
* @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public double getZ() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
/**
* Returns the axis in the axis-angle representation of this rotation.
*
* @return The axis in the axis-angle representation.
*/
public Vector<N3> getAxis() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
}
/**
* Returns the angle in radians in the axis-angle representation of this rotation.
*
* @return The angle in radians in the axis-angle representation of this rotation.
*/
public double getAngle() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return 2.0 * Math.atan2(norm, m_q.getW());
}
/**
* Checks equality between this Rotation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation3d) {
var other = (Rotation3d) obj;
return m_q.equals(other.m_q);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_q);
}
@Override
@SuppressWarnings("ParameterName")
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
@Override
public String toString() {
return String.format("Rotation3d(%s)", m_q);
}
}

165
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -1,165 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import java.util.Objects;
/** Represents a transformation for a Pose3d. */
public class Transform3d {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform3d(Pose3d initial, Pose3d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
public Transform3d times(double scalar) {
return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
public Transform3d plus(Transform3d other) {
return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
public Transform3d inverse() {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return new Transform3d(
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
getRotation().unaryMinus());
}
@Override
public String toString() {
return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform3d) {
return ((Transform3d) obj).m_translation.equals(m_translation)
&& ((Transform3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

247
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -1,247 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 3D space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes. When the robot is at the
* origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
* positive Z.
*/
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation3d implements Interpolatable<Translation3d> {
private final double m_x;
private final double m_y;
private final double m_z;
/** Constructs a Translation3d with X, Y, and Z components equal to zero. */
public Translation3d() {
this(0.0, 0.0, 0.0);
}
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
@JsonCreator
public Translation3d(
@JsonProperty(required = true, value = "x") double x,
@JsonProperty(required = true, value = "y") double y,
@JsonProperty(required = true, value = "z") double z) {
m_x = x;
m_y = y;
m_z = z;
}
/**
* Constructs a Translation3d with the provided distance and angle. This is essentially converting
* from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
public Translation3d(double distance, Rotation3d angle) {
final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
m_x = rectangular.getX();
m_y = rectangular.getY();
m_z = rectangular.getZ();
}
/**
* Calculates the distance between two translations in 3D space.
*
* <p>The distance between translations is defined as √((x2−x1)²+(y2−y1)²+(z2−z1)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation3d other) {
return Math.sqrt(
Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
}
/**
* Returns the X component of the translation.
*
* @return The X component of the translation.
*/
@JsonProperty
public double getX() {
return m_x;
}
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
@JsonProperty
public double getY() {
return m_y;
}
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
@JsonProperty
public double getZ() {
return m_z;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
*
* <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
* will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateBy(Rotation3d other) {
final var p = new Quaternion(0.0, m_x, m_y, m_z);
final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Returns the sum of two translations in 3D space.
*
* <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
* Translation3d{3.0, 8.0, 11.0).
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation3d plus(Translation3d other) {
return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
}
/**
* Returns the difference between two translations.
*
* <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
* Translation3d(4.0, 2.0, 0.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation3d minus(Translation3d other) {
return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
}
/**
* Returns the inverse of the current translation. This is equivalent to negating all components
* of the translation.
*
* @return The inverse of the current translation.
*/
public Translation3d unaryMinus() {
return new Translation3d(-m_x, -m_y, -m_z);
}
/**
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation3d times(double scalar) {
return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
}
/**
* Returns the translation divided by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation3d div(double scalar) {
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
}
/**
* Checks equality between this Translation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation3d) {
return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
&& Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y, m_z);
}
@Override
public Translation3d interpolate(Translation3d endValue, double t) {
return new Translation3d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*
* @return A Translation2d representing this Translation3d projected into the X-Y plane.
*/
public Translation2d toTranslation2d() {
return new Translation2d(m_x, m_y);
}
}

99
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
View File

@@ -1,99 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import java.util.Objects;
/**
* A change in distance along a 3D arc since the last pose update. We can use ideas from
* differential calculus to create new Pose3ds from a Twist3d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist3d {
/** Linear "dx" component. */
public double dx;
/** Linear "dy" component. */
public double dy;
/** Linear "dz" component. */
public double dz;
/** Angular "droll" component (radians). */
public double droll;
/** Angular "dpitch" component (radians). */
public double dpitch;
/** Angular "dyaw" component (radians). */
public double dyaw;
public Twist3d() {}
/**
* Constructs a Twist3d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dz Change in z direction relative to robot.
* @param droll Change in roll relative to the robot.
* @param dpitch Change in pitch relative to the robot.
* @param dyaw Change in yaw relative to the robot.
*/
public Twist3d(double dx, double dy, double dz, double droll, double dpitch, double dyaw) {
this.dx = dx;
this.dy = dy;
this.dz = dz;
this.droll = droll;
this.dpitch = dpitch;
this.dyaw = dyaw;
}
@Override
public String toString() {
return String.format(
"Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, dRoll: %.2f, dPitch: %.2f, dYaw: %.2f)",
dx, dy, dz, droll, dpitch, dyaw);
}
/**
* Checks equality between this Twist3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist3d) {
return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist3d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist3d) obj).dz - dz) < 1E-9
&& Math.abs(((Twist3d) obj).droll - droll) < 1E-9
&& Math.abs(((Twist3d) obj).dpitch - dpitch) < 1E-9
&& Math.abs(((Twist3d) obj).dyaw - dyaw) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dz, droll, dpitch, dyaw);
}
}

113
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java Normal file
View File

@@ -0,0 +1,113 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.networktables;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.BooleanTopic;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.IntegerTopic;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.RawPublisher;
/**
* 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.
*
* <p>However, we do expect that the actual logic which fills out values in the entries will be
* different for sim vs. real camera
*/
public class NTTopicSet {
public NetworkTable subTable;
public RawPublisher rawBytesEntry;
public IntegerTopic pipelineIndexTopic;
public IntegerPublisher pipelineIndexPublisher;
public IntegerSubscriber pipelineIndexSubscriber;
public BooleanTopic driverModeEntry;
public BooleanPublisher driverModePublisher;
public BooleanSubscriber driverModeSubscriber;
public DoublePublisher latencyMillisEntry;
public BooleanPublisher hasTargetEntry;
public DoublePublisher targetPitchEntry;
public DoublePublisher targetYawEntry;
public DoublePublisher targetAreaEntry;
public DoubleArrayPublisher targetPoseEntry;
public DoublePublisher targetSkewEntry;
// The raw position of the best target, in pixels.
public DoublePublisher bestTargetPosX;
public DoublePublisher bestTargetPosY;
// Heartbeat
public IntegerTopic heartbeatTopic;
public IntegerPublisher heartbeatPublisher;
public void updateEntries() {
rawBytesEntry = subTable.getRawTopic("rawBytes").publish("rawBytes");
pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
pipelineIndexPublisher = pipelineIndexTopic.publish();
pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0);
driverModeEntry = subTable.getBooleanTopic("driverMode");
driverModePublisher = driverModeEntry.publish();
driverModeSubscriber = driverModeEntry.subscribe(false);
latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
targetPoseEntry = subTable.getDoubleArrayTopic("targetPose").publish();
targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();
bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();
bestTargetPosY = subTable.getDoubleTopic("targetPixelsY").publish();
heartbeatTopic = subTable.getIntegerTopic("heartbeat");
heartbeatPublisher = heartbeatTopic.publish();
}
@SuppressWarnings("DuplicatedCode")
public void removeEntries() {
if (rawBytesEntry != null) rawBytesEntry.close();
if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
if (pipelineIndexSubscriber != null) pipelineIndexSubscriber.close();
if (driverModePublisher != null) driverModePublisher.close();
if (driverModeSubscriber != null) driverModeSubscriber.close();
if (latencyMillisEntry != null) latencyMillisEntry.close();
if (hasTargetEntry != null) hasTargetEntry.close();
if (targetPitchEntry != null) targetPitchEntry.close();
if (targetAreaEntry != null) targetAreaEntry.close();
if (targetYawEntry != null) targetYawEntry.close();
if (targetPoseEntry != null) targetPoseEntry.close();
if (targetSkewEntry != null) targetSkewEntry.close();
if (bestTargetPosX != null) bestTargetPosX.close();
if (bestTargetPosY != null) bestTargetPosY.close();
}
}

1
photonlib-cpp-examples/aimandrange/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
vendordeps

6
photonlib-cpp-examples/aimandrange/.wpilib/wpilib_preferences.json Normal file
View File

@@ -0,0 +1,6 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
"projectYear": "2023Beta",
"teamNumber": 5
}

24
photonlib-cpp-examples/aimandrange/WPILib-License.md Normal file
View File

@@ -0,0 +1,24 @@
Copyright (c) 2009-2021 FIRST and other WPILib contributors
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

116
photonlib-cpp-examples/aimandrange/build.gradle Normal file
View File

@@ -0,0 +1,116 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
id "com.dorongold.task-tree" version "2.1.0"
}
repositories {
mavenLocal()
jcenter()
}
apply from: "${rootDir}/../shared/examples_common.gradle"
// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
targets {
roborio(getTargetTypeClass('RoboRIO')) {
// Team number is loaded either from the .wpilib/wpilib_preferences.json
// or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file.
team = project.frc.getTeamOrDefault(5940)
debug = project.frc.getDebugOrDefault(false)
artifacts {
// First part is artifact name, 2nd is artifact type
// getTargetTypeClass is a shortcut to get the class type using a string
frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
}
// Static files artifact
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy'
}
}
}
}
}
def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
// Set this to true to enable desktop support.
def includeDesktopSupport = true
// Set to true to run simulation in debug mode
wpi.cpp.debugSimulation = false
// Default enable simgui
wpi.sim.addGui().defaultEnabled = true
// Enable DS but not by default
wpi.sim.addDriverstation()
model {
components {
frcUserProgram(NativeExecutableSpec) {
// We don't need to build for roborio -- if we do, we need to install
// a roborio toolchain every time we build in CI
// Ideally, we'd be able to set the roborio toolchain as optional, but
// I can't figure out how to set that environment variable from build.gradle
// (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
// for now, commented out
// targetPlatform wpi.platforms.roborio
if (includeDesktopSupport) {
targetPlatform wpi.platforms.desktop
}
sources.cpp {
source {
srcDir 'src/main/cpp'
include '**/*.cpp', '**/*.cc'
}
exportedHeaders {
srcDir 'src/main/include'
}
}
// Set deploy task to deploy this component
deployArtifact.component = it
// Enable run tasks for this component
wpi.cpp.enableExternalTasks(it)
// Enable simulation for this component
wpi.sim.enable(it)
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
wpi.cpp.vendor.cpp(it)
wpi.cpp.deps.wpilib(it)
}
}
testSuites {
frcUserProgramTest(GoogleTestTestSuiteSpec) {
testing $.components.frcUserProgram
sources.cpp {
source {
srcDir 'src/test/cpp'
include '**/*.cpp'
}
}
// Enable run tasks for this component
wpi.cpp.enableExternalTasks(it)
wpi.cpp.vendor.cpp(it)
wpi.cpp.deps.wpilib(it)
wpi.cpp.deps.googleTest(it)
}
}
}

240
photonlib-cpp-examples/aimandrange/gradlew vendored Executable file
View File

@@ -0,0 +1,240 @@
#!/bin/sh
#
# Copyright © 2015-2021 the original authors.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
##############################################################################
#
# Gradle start up script for POSIX generated by Gradle.
#
# Important for running:
#
# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
# noncompliant, but you have some other compliant shell such as ksh or
# bash, then to run this script, type that shell name before the whole
# command line, like:
#
# ksh Gradle
#
# Busybox and similar reduced shells will NOT work, because this script
# requires all of these POSIX shell features:
# * functions;
# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
# * compound commands having a testable exit status, especially «case»;
# * various built-in commands including «command», «set», and «ulimit».
#
# Important for patching:
#
# (2) This script targets any POSIX shell, so it avoids extensions provided
# by Bash, Ksh, etc; in particular arrays are avoided.
#
# The "traditional" practice of packing multiple parameters into a
# space-separated string is a well documented source of bugs and security
# problems, so this is (mostly) avoided, by progressively accumulating
# options in "$@", and eventually passing that to Java.
#
# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
# see the in-line comments for details.
#
# There are tweaks for specific operating systems such as AIX, CygWin,
# Darwin, MinGW, and NonStop.
#
# (3) This script is generated from the Groovy template
# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
# within the Gradle project.
#
# You can find Gradle at https://github.com/gradle/gradle/.
#
##############################################################################
# Attempt to set APP_HOME
# Resolve links: $0 may be a link
app_path=$0
# Need this for daisy-chained symlinks.
while
APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
[ -h "$app_path" ]
do
ls=$( ls -ld "$app_path" )
link=${ls#*' -> '}
case $link in #(
/*) app_path=$link ;; #(
*) app_path=$APP_HOME$link ;;
esac
done
APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
APP_NAME="Gradle"
APP_BASE_NAME=${0##*/}
# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
# Use the maximum available, or set MAX_FD != -1 to use that value.
MAX_FD=maximum
warn () {
echo "$*"
} >&2
die () {
echo
echo "$*"
echo
exit 1
} >&2
# OS specific support (must be 'true' or 'false').
cygwin=false
msys=false
darwin=false
nonstop=false
case "$( uname )" in #(
CYGWIN* ) cygwin=true ;; #(
Darwin* ) darwin=true ;; #(
MSYS* | MINGW* ) msys=true ;; #(
NONSTOP* ) nonstop=true ;;
esac
CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
# Determine the Java command to use to start the JVM.
if [ -n "$JAVA_HOME" ] ; then
if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
# IBM's JDK on AIX uses strange locations for the executables
JAVACMD=$JAVA_HOME/jre/sh/java
else
JAVACMD=$JAVA_HOME/bin/java
fi
if [ ! -x "$JAVACMD" ] ; then
die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
fi
else
JAVACMD=java
which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
fi
# Increase the maximum file descriptors if we can.
if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
case $MAX_FD in #(
max*)
MAX_FD=$( ulimit -H -n ) ||
warn "Could not query maximum file descriptor limit"
esac
case $MAX_FD in #(
'' | soft) :;; #(
*)
ulimit -n "$MAX_FD" ||
warn "Could not set maximum file descriptor limit to $MAX_FD"
esac
fi
# Collect all arguments for the java command, stacking in reverse order:
# * args from the command line
# * the main class name
# * -classpath
# * -D...appname settings
# * --module-path (only if needed)
# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
# For Cygwin or MSYS, switch paths to Windows format before running java
if "$cygwin" || "$msys" ; then
APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
JAVACMD=$( cygpath --unix "$JAVACMD" )
# Now convert the arguments - kludge to limit ourselves to /bin/sh
for arg do
if
case $arg in #(
-*) false ;; # don't mess with options #(
/?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
[ -e "$t" ] ;; #(
*) false ;;
esac
then
arg=$( cygpath --path --ignore --mixed "$arg" )
fi
# Roll the args list around exactly as many times as the number of
# args, so each arg winds up back in the position where it started, but
# possibly modified.
#
# NB: a `for` loop captures its iteration list before it begins, so
# changing the positional parameters here affects neither the number of
# iterations, nor the values presented in `arg`.
shift # remove old arg
set -- "$@" "$arg" # push replacement arg
done
fi
# Collect all arguments for the java command;
# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
# shell script including quotes and variable substitutions, so put them in
# double quotes to make sure that they get re-expanded; and
# * put everything else in single quotes, so that it's not re-expanded.
set -- \
"-Dorg.gradle.appname=$APP_BASE_NAME" \
-classpath "$CLASSPATH" \
org.gradle.wrapper.GradleWrapperMain \
"$@"
# Stop when "xargs" is not available.
if ! command -v xargs >/dev/null 2>&1
then
die "xargs is not available"
fi
# Use "xargs" to parse quoted args.
#
# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
#
# In Bash we could simply go:
#
# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
# set -- "${ARGS[@]}" "$@"
#
# but POSIX shell has neither arrays nor command substitution, so instead we
# post-process each arg (as a line of input to sed) to backslash-escape any
# character that might be a shell metacharacter, then use eval to reverse
# that process (while maintaining the separation between arguments), and wrap
# the whole thing up as a single "set" statement.
#
# This will of course break if any of these variables contains a newline or
# an unmatched quote.
#
eval "set -- $(
printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
xargs -n1 |
sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
tr '\n' ' '
)" '"$@"'
exec "$JAVACMD" "$@"

91
photonlib-cpp-examples/aimandrange/gradlew.bat vendored Normal file
View File

@@ -0,0 +1,91 @@
@rem
@rem Copyright 2015 the original author or authors.
@rem
@rem Licensed under the Apache License, Version 2.0 (the "License");
@rem you may not use this file except in compliance with the License.
@rem You may obtain a copy of the License at
@rem
@rem https://www.apache.org/licenses/LICENSE-2.0
@rem
@rem Unless required by applicable law or agreed to in writing, software
@rem distributed under the License is distributed on an "AS IS" BASIS,
@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@rem See the License for the specific language governing permissions and
@rem limitations under the License.
@rem
@if "%DEBUG%"=="" @echo off
@rem ##########################################################################
@rem
@rem Gradle startup script for Windows
@rem
@rem ##########################################################################
@rem Set local scope for the variables with windows NT shell
if "%OS%"=="Windows_NT" setlocal
set DIRNAME=%~dp0
if "%DIRNAME%"=="" set DIRNAME=.
set APP_BASE_NAME=%~n0
set APP_HOME=%DIRNAME%
@rem Resolve any "." and ".." in APP_HOME to make it shorter.
for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
@rem Find java.exe
if defined JAVA_HOME goto findJavaFromJavaHome
set JAVA_EXE=java.exe
%JAVA_EXE% -version >NUL 2>&1
if %ERRORLEVEL% equ 0 goto execute
echo.
echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.
goto fail
:findJavaFromJavaHome
set JAVA_HOME=%JAVA_HOME:"=%
set JAVA_EXE=%JAVA_HOME%/bin/java.exe
if exist "%JAVA_EXE%" goto execute
echo.
echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.
goto fail
:execute
@rem Setup the command line
set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
@rem Execute Gradle
"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
:end
@rem End local scope for the variables with windows NT shell
if %ERRORLEVEL% equ 0 goto mainEnd
:fail
rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
rem the _cmd.exe /c_ return code!
set EXIT_CODE=%ERRORLEVEL%
if %EXIT_CODE% equ 0 set EXIT_CODE=1
if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
exit /b %EXIT_CODE%
:mainEnd
if "%OS%"=="Windows_NT" endlocal
:omega

30
photonlib-cpp-examples/aimandrange/settings.gradle Normal file
View File

@@ -0,0 +1,30 @@
import org.gradle.internal.os.OperatingSystem
rootProject.name = 'aimandrange'
pluginManagement {
repositories {
mavenLocal()
jcenter()
gradlePluginPortal()
String frcYear = '2023'
File frcHome
if (OperatingSystem.current().isWindows()) {
String publicFolder = System.getenv('PUBLIC')
if (publicFolder == null) {
publicFolder = "C:\\Users\\Public"
}
def homeRoot = new File(publicFolder, "wpilib")
frcHome = new File(homeRoot, frcYear)
} else {
def userFolder = System.getProperty("user.home")
def homeRoot = new File(userFolder, "wpilib")
frcHome = new File(homeRoot, frcYear)
}
def frcHomeMaven = new File(frcHome, 'maven')
maven {
name 'frcHome'
url frcHomeMaven
}
}
}

97
photonlib-cpp-examples/aimandrange/simgui-ds.json Normal file
View File

@@ -0,0 +1,97 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 6,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}

60
photonlib-cpp-examples/aimandrange/simgui-window.json Normal file
View File

@@ -0,0 +1,60 @@
{
"MainWindow": {
"GLOBAL": {
"height": "720",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "1733",
"xpos": "3306",
"ypos": "476"
}
},
"Window": {
"###FMS": {
"Collapsed": "0",
"Pos": "5,540",
"Size": "283,146"
},
"###Joysticks": {
"Collapsed": "0",
"Pos": "255,463",
"Size": "796,155"
},
"###Keyboard 0 Settings": {
"Collapsed": "0",
"Pos": "10,50",
"Size": "300,560"
},
"###NetworkTables": {
"Collapsed": "0",
"Pos": "367,58",
"Size": "750,386"
},
"###Other Devices": {
"Collapsed": "0",
"Pos": "1025,20",
"Size": "250,695"
},
"###System Joysticks": {
"Collapsed": "0",
"Pos": "15,306",
"Size": "192,218"
},
"###Timing": {
"Collapsed": "0",
"Pos": "5,150",
"Size": "135,127"
},
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Robot State": {
"Collapsed": "0",
"Pos": "5,20",
"Size": "92,99"
}
}
}

17
photonlib-cpp-examples/aimandrange/simgui.json Normal file
View File

@@ -0,0 +1,17 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo"
}
},
"NetworkTables": {
"transitory": {
"photonvision": {
"open": true,
"photonvision": {
"open": true
}
}
}
}
}

0
photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp → photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp
View File

0
photonlib-cpp-examples/src/main/cpp/examples/aimandrange/include/Robot.h → photonlib-cpp-examples/aimandrange/src/main/include/Robot.h
View File

11
photonlib-java-examples/src/dev/native/cpp/main.cpp → photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp
View File

@@ -22,4 +22,13 @@
* SOFTWARE.
*/
int main() {}
#include <hal/HAL.h>
#include "gtest/gtest.h"
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

1
photonlib-cpp-examples/aimattarget/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
vendordeps

6
photonlib-cpp-examples/aimattarget/.wpilib/wpilib_preferences.json Normal file
View File

@@ -0,0 +1,6 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
"projectYear": "2023Beta",
"teamNumber": 5
}

24
photonlib-cpp-examples/aimattarget/WPILib-License.md Normal file
View File

@@ -0,0 +1,24 @@
Copyright (c) 2009-2021 FIRST and other WPILib contributors
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

116
photonlib-cpp-examples/aimattarget/build.gradle Normal file
View File

@@ -0,0 +1,116 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
id "com.dorongold.task-tree" version "2.1.0"
}
repositories {
mavenLocal()
jcenter()
}
apply from: "${rootDir}/../shared/examples_common.gradle"
// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
targets {
roborio(getTargetTypeClass('RoboRIO')) {
// Team number is loaded either from the .wpilib/wpilib_preferences.json
// or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file.
team = project.frc.getTeamOrDefault(5940)
debug = project.frc.getDebugOrDefault(false)
artifacts {
// First part is artifact name, 2nd is artifact type
// getTargetTypeClass is a shortcut to get the class type using a string
frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
}
// Static files artifact
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy'
}
}
}
}
}
def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
// Set this to true to enable desktop support.
def includeDesktopSupport = true
// Set to true to run simulation in debug mode
wpi.cpp.debugSimulation = false
// Default enable simgui
wpi.sim.addGui().defaultEnabled = true
// Enable DS but not by default
wpi.sim.addDriverstation()
model {
components {
frcUserProgram(NativeExecutableSpec) {
// We don't need to build for roborio -- if we do, we need to install
// a roborio toolchain every time we build in CI
// Ideally, we'd be able to set the roborio toolchain as optional, but
// I can't figure out how to set that environment variable from build.gradle
// (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
// for now, commented out
// targetPlatform wpi.platforms.roborio
if (includeDesktopSupport) {
targetPlatform wpi.platforms.desktop
}
sources.cpp {
source {
srcDir 'src/main/cpp'
include '**/*.cpp', '**/*.cc'
}
exportedHeaders {
srcDir 'src/main/include'
}
}
// Set deploy task to deploy this component
deployArtifact.component = it
// Enable run tasks for this component
wpi.cpp.enableExternalTasks(it)
// Enable simulation for this component
wpi.sim.enable(it)
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
wpi.cpp.vendor.cpp(it)
wpi.cpp.deps.wpilib(it)
}
}
testSuites {
frcUserProgramTest(GoogleTestTestSuiteSpec) {
testing $.components.frcUserProgram
sources.cpp {
source {
srcDir 'src/test/cpp'
include '**/*.cpp'
}
}
// Enable run tasks for this component
wpi.cpp.enableExternalTasks(it)
wpi.cpp.vendor.cpp(it)
wpi.cpp.deps.wpilib(it)
wpi.cpp.deps.googleTest(it)
}
}
}

240
photonlib-cpp-examples/aimattarget/gradlew vendored Executable file
View File

@@ -0,0 +1,240 @@
#!/bin/sh
#
# Copyright © 2015-2021 the original authors.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
##############################################################################
#
# Gradle start up script for POSIX generated by Gradle.
#
# Important for running:
#
# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
# noncompliant, but you have some other compliant shell such as ksh or
# bash, then to run this script, type that shell name before the whole
# command line, like:
#
# ksh Gradle
#
# Busybox and similar reduced shells will NOT work, because this script
# requires all of these POSIX shell features:
# * functions;
# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
# * compound commands having a testable exit status, especially «case»;
# * various built-in commands including «command», «set», and «ulimit».
#
# Important for patching:
#
# (2) This script targets any POSIX shell, so it avoids extensions provided
# by Bash, Ksh, etc; in particular arrays are avoided.
#
# The "traditional" practice of packing multiple parameters into a
# space-separated string is a well documented source of bugs and security
# problems, so this is (mostly) avoided, by progressively accumulating
# options in "$@", and eventually passing that to Java.
#
# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
# see the in-line comments for details.
#
# There are tweaks for specific operating systems such as AIX, CygWin,
# Darwin, MinGW, and NonStop.
#
# (3) This script is generated from the Groovy template
# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
# within the Gradle project.
#
# You can find Gradle at https://github.com/gradle/gradle/.
#
##############################################################################
# Attempt to set APP_HOME
# Resolve links: $0 may be a link
app_path=$0
# Need this for daisy-chained symlinks.
while
APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
[ -h "$app_path" ]
do
ls=$( ls -ld "$app_path" )
link=${ls#*' -> '}
case $link in #(
/*) app_path=$link ;; #(
*) app_path=$APP_HOME$link ;;
esac
done
APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
APP_NAME="Gradle"
APP_BASE_NAME=${0##*/}
# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
# Use the maximum available, or set MAX_FD != -1 to use that value.
MAX_FD=maximum
warn () {
echo "$*"
} >&2
die () {
echo
echo "$*"
echo
exit 1
} >&2
# OS specific support (must be 'true' or 'false').
cygwin=false
msys=false
darwin=false
nonstop=false
case "$( uname )" in #(
CYGWIN* ) cygwin=true ;; #(
Darwin* ) darwin=true ;; #(
MSYS* | MINGW* ) msys=true ;; #(
NONSTOP* ) nonstop=true ;;
esac
CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
# Determine the Java command to use to start the JVM.
if [ -n "$JAVA_HOME" ] ; then
if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
# IBM's JDK on AIX uses strange locations for the executables
JAVACMD=$JAVA_HOME/jre/sh/java
else
JAVACMD=$JAVA_HOME/bin/java
fi
if [ ! -x "$JAVACMD" ] ; then
die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
fi
else
JAVACMD=java
which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
fi
# Increase the maximum file descriptors if we can.
if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
case $MAX_FD in #(
max*)
MAX_FD=$( ulimit -H -n ) ||
warn "Could not query maximum file descriptor limit"
esac
case $MAX_FD in #(
'' | soft) :;; #(
*)
ulimit -n "$MAX_FD" ||
warn "Could not set maximum file descriptor limit to $MAX_FD"
esac
fi
# Collect all arguments for the java command, stacking in reverse order:
# * args from the command line
# * the main class name
# * -classpath
# * -D...appname settings
# * --module-path (only if needed)
# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
# For Cygwin or MSYS, switch paths to Windows format before running java
if "$cygwin" || "$msys" ; then
APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
JAVACMD=$( cygpath --unix "$JAVACMD" )
# Now convert the arguments - kludge to limit ourselves to /bin/sh
for arg do
if
case $arg in #(
-*) false ;; # don't mess with options #(
/?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
[ -e "$t" ] ;; #(
*) false ;;
esac
then
arg=$( cygpath --path --ignore --mixed "$arg" )
fi
# Roll the args list around exactly as many times as the number of
# args, so each arg winds up back in the position where it started, but
# possibly modified.
#
# NB: a `for` loop captures its iteration list before it begins, so
# changing the positional parameters here affects neither the number of
# iterations, nor the values presented in `arg`.
shift # remove old arg
set -- "$@" "$arg" # push replacement arg
done
fi
# Collect all arguments for the java command;
# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
# shell script including quotes and variable substitutions, so put them in
# double quotes to make sure that they get re-expanded; and
# * put everything else in single quotes, so that it's not re-expanded.
set -- \
"-Dorg.gradle.appname=$APP_BASE_NAME" \
-classpath "$CLASSPATH" \
org.gradle.wrapper.GradleWrapperMain \
"$@"
# Stop when "xargs" is not available.
if ! command -v xargs >/dev/null 2>&1
then
die "xargs is not available"
fi
# Use "xargs" to parse quoted args.
#
# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
#
# In Bash we could simply go:
#
# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
# set -- "${ARGS[@]}" "$@"
#
# but POSIX shell has neither arrays nor command substitution, so instead we
# post-process each arg (as a line of input to sed) to backslash-escape any
# character that might be a shell metacharacter, then use eval to reverse
# that process (while maintaining the separation between arguments), and wrap
# the whole thing up as a single "set" statement.
#
# This will of course break if any of these variables contains a newline or
# an unmatched quote.
#
eval "set -- $(
printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
xargs -n1 |
sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
tr '\n' ' '
)" '"$@"'
exec "$JAVACMD" "$@"

91
photonlib-cpp-examples/aimattarget/gradlew.bat vendored Normal file
View File

@@ -0,0 +1,91 @@
@rem
@rem Copyright 2015 the original author or authors.
@rem
@rem Licensed under the Apache License, Version 2.0 (the "License");
@rem you may not use this file except in compliance with the License.
@rem You may obtain a copy of the License at
@rem
@rem https://www.apache.org/licenses/LICENSE-2.0
@rem
@rem Unless required by applicable law or agreed to in writing, software
@rem distributed under the License is distributed on an "AS IS" BASIS,
@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@rem See the License for the specific language governing permissions and
@rem limitations under the License.
@rem
@if "%DEBUG%"=="" @echo off
@rem ##########################################################################
@rem
@rem Gradle startup script for Windows
@rem
@rem ##########################################################################
@rem Set local scope for the variables with windows NT shell
if "%OS%"=="Windows_NT" setlocal
set DIRNAME=%~dp0
if "%DIRNAME%"=="" set DIRNAME=.
set APP_BASE_NAME=%~n0
set APP_HOME=%DIRNAME%
@rem Resolve any "." and ".." in APP_HOME to make it shorter.
for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
@rem Find java.exe
if defined JAVA_HOME goto findJavaFromJavaHome
set JAVA_EXE=java.exe
%JAVA_EXE% -version >NUL 2>&1
if %ERRORLEVEL% equ 0 goto execute
echo.
echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.
goto fail
:findJavaFromJavaHome
set JAVA_HOME=%JAVA_HOME:"=%
set JAVA_EXE=%JAVA_HOME%/bin/java.exe
if exist "%JAVA_EXE%" goto execute
echo.
echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.
goto fail
:execute
@rem Setup the command line
set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
@rem Execute Gradle
"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
:end
@rem End local scope for the variables with windows NT shell
if %ERRORLEVEL% equ 0 goto mainEnd
:fail
rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
rem the _cmd.exe /c_ return code!
set EXIT_CODE=%ERRORLEVEL%
if %EXIT_CODE% equ 0 set EXIT_CODE=1
if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
exit /b %EXIT_CODE%
:mainEnd
if "%OS%"=="Windows_NT" endlocal
:omega

30
photonlib-cpp-examples/aimattarget/settings.gradle Normal file
View File

@@ -0,0 +1,30 @@
import org.gradle.internal.os.OperatingSystem
rootProject.name = 'aimattarget'
pluginManagement {
repositories {
mavenLocal()
jcenter()
gradlePluginPortal()
String frcYear = '2023'
File frcHome
if (OperatingSystem.current().isWindows()) {
String publicFolder = System.getenv('PUBLIC')
if (publicFolder == null) {
publicFolder = "C:\\Users\\Public"
}
def homeRoot = new File(publicFolder, "wpilib")
frcHome = new File(homeRoot, frcYear)
} else {
def userFolder = System.getProperty("user.home")
def homeRoot = new File(userFolder, "wpilib")
frcHome = new File(homeRoot, frcYear)
}
def frcHomeMaven = new File(frcHome, 'maven')
maven {
name 'frcHome'
url frcHomeMaven
}
}
}

97
photonlib-cpp-examples/aimattarget/simgui-ds.json Normal file
View File

@@ -0,0 +1,97 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 6,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}

60
photonlib-cpp-examples/aimattarget/simgui-window.json Normal file
View File

@@ -0,0 +1,60 @@
{
"MainWindow": {
"GLOBAL": {
"height": "720",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "1280",
"xpos": "3124",
"ypos": "324"
}
},
"Window": {
"###FMS": {
"Collapsed": "0",
"Pos": "5,540",
"Size": "283,146"
},
"###Joysticks": {
"Collapsed": "0",
"Pos": "250,465",
"Size": "796,155"
},
"###Keyboard 0 Settings": {
"Collapsed": "0",
"Pos": "10,50",
"Size": "300,560"
},
"###NetworkTables": {
"Collapsed": "0",
"Pos": "250,277",
"Size": "750,185"
},
"###Other Devices": {
"Collapsed": "0",
"Pos": "1025,20",
"Size": "250,695"
},
"###System Joysticks": {
"Collapsed": "0",
"Pos": "5,350",
"Size": "192,218"
},
"###Timing": {
"Collapsed": "0",
"Pos": "5,150",
"Size": "135,127"
},
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Robot State": {
"Collapsed": "0",
"Pos": "5,20",
"Size": "92,99"
}
}
}

7
photonlib-cpp-examples/aimattarget/simgui.json Normal file
View File

@@ -0,0 +1,7 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo"
}
}
}

0
photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp → photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp
View File

0
photonlib-cpp-examples/src/main/cpp/examples/aimattarget/include/Robot.h → photonlib-cpp-examples/aimattarget/src/main/include/Robot.h
View File

34
photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp Normal file
View File

@@ -0,0 +1,34 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <hal/HAL.h>
#include "gtest/gtest.h"
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

Some files were not shown because too many files have changed in this diff Show More