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

[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)

Browse Source 
* add classes to targeting and update gradle

* rename me

* Finish cleanup

* Formatting fixes

* just use common.gradle

* Update build.gradle

* Update config.gradle

* remove typo

* simplify

* Add Packet Headers

* move simulation classes into simulation folder

* draw in dependency

* fix

* Everything working minus tests cause im lazy

* formatting fixes

REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN

* move packet test to targeting

* Formatting fixes

* remove TargetCorner from c++

im not 100% sure but just doing std::pair<double, double> is sufficient and shouldnt conflict with protobuf

* think i added packet

* fix namespace issue

* organize imports in photon-targeting

* Formatting fixes

* remove ctors

* fix typo

* Add PNP and Multitag packet tests

* revert TargetCorner class

* Add Test placeholders

* remove annoying print

* Reorganize build and publish process

channeling inner Thad

* add targeting as flag

* Update config.gradle

* fix issue with platform binaries not building

* Update photonlib.json.in

casing still needs to be checked

* add minimum level back

* add back UTF-8 encoding of javadoc

* simplify publish model for photon-lib

* fix windows symbol generation

* formatting fixes

* move task from main gradle to config

* Update config.gradle
This commit is contained in:
Sriman Achanta
2023-11-19 15:16:22 -05:00
committed by GitHub
parent 308fd801d4
commit 994ea1e76b
50 changed files with 1132 additions and 910 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
photon-lib/build.gradle
View File

@@ -1,111 +1,73 @@
plugins {
id "java"
id 'cpp'
id "google-test-test-suite"
id 'edu.wpi.first.WpilibTools' version '1.3.0'
}
import java.nio.file.Path
apply plugin: "edu.wpi.first.NativeUtils"
apply from: "${rootDir}/shared/config.gradle"
import java.nio.file.Path
ext {
nativeName = "photonlib"
includePhotonTargeting = true
}
apply from: "${rootDir}/shared/setupBuild.gradle"
apply from: "${rootDir}/versioningHelper.gradle"
test {
useJUnitPlatform()
}
java {
sourceCompatibility = JavaVersion.VERSION_11
targetCompatibility = JavaVersion.VERSION_11
}
wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
println("Building for WPILib ${wpilibTools.deps.wpilibVersion}")
// Apply Java configuration
def nativeConfigName = 'wpilibNatives'
def nativeConfig = configurations.create(nativeConfigName)
def nativeTasks = wpilibTools.createExtractionTasks {
configurationName = nativeConfigName
}
nativeTasks.addToSourceSetResources(sourceSets.main)
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
dependencies {
implementation project(":photon-targeting")
// WPILib deps
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("cameraserver")
implementation wpilibTools.deps.wpilibJava("cscore")
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("wpinet")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation wpilibTools.deps.wpilibJava("apriltag")
implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
// Jackson
implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get()
implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get()
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()
implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get()
// Junit
testImplementation wpilibTools.deps.wpilibJava("cscore")
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
}
// Set up exports properly
nativeUtils {
exportsConfigs {
// Main library is just default empty. This will export everything
Photon {
}
}
}
model {
components {
Photon(NativeLibrarySpec) {
sources {
cpp {
source {
srcDirs "src/main/native/cpp"
include "**/*.cpp"
}
exportedHeaders {
srcDirs "src/main/native/include"
srcDirs "src/generate/native/include"
}
}
}
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
testSuites {
cppTest(GoogleTestTestSuiteSpec) {
testing $.components.Photon
sources.cpp {
source {
srcDir "src/test/native/cpp"
include "**/*.cpp"
}
}
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "googletest_static")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
cppHeadersZip {
from('src/generate/native/include') {
into '/'
}
}
def photonlibFileInput = file("src/generate/photonlib.json.in")
ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json")
def vendorJson = artifacts.add('archives', file("$photonlibFileOutput"))
task generateVendorJson() {
description = "Generates the vendor JSON file"
group = "PhotonVision"
@@ -141,29 +103,28 @@ task writeCurrentVersion {
build.mustRunAfter writeCurrentVersion
tasks.withType(Javadoc) {
options.encoding = 'UTF-8'
model {
components {
all {
it.sources.each {
it.exportedHeaders {
srcDirs "src/main/native/include"
srcDirs "src/generate/native/include"
}
}
}
}
// Publish the vendordep json
publishing {
publications {
vendorjson(MavenPublication) {
artifact vendorJson
artifactId = "${nativeName}-json"
groupId = "org.photonvision"
version "1.0"
}
}
}
}
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("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("wpimath")
testNativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())

206
photon-lib/publish.gradle
View File

@@ -1,206 +0,0 @@
apply plugin: 'maven-publish'
ext.licenseFile = files("$rootDir/LICENSE")
ext.photonVersionFile = files("$projectDir/src/generate/native/include")
def outputsFolder = file("$buildDir/outputs")
def allOutputsFolder = file("$buildDir/allOutputs")
def versionFile = file("$allOutputsFolder/version.txt")
task outputVersions() {
description = 'Prints the versions of wpilib to a file for use by the downstream packaging project'
group = 'Build'
outputs.files(versionFile)
doFirst {
buildDir.mkdir()
outputsFolder.mkdir()
allOutputsFolder.mkdir()
}
doLast {
versionFile.write pubVersion
}
}
task libraryBuild() {}
build.dependsOn outputVersions
task copyAllOutputs(type: Copy) {
destinationDir allOutputsFolder
}
build.dependsOn copyAllOutputs
copyAllOutputs.dependsOn outputVersions
ext.addTaskToCopyAllOutputs = { task ->
copyAllOutputs.dependsOn task
copyAllOutputs.inputs.file task.archiveFile
copyAllOutputs.from task.archiveFile
}
def artifactGroupId = 'org.photonvision'
def baseArtifactId = 'PhotonLib'
def zipBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-cpp_CLS"
def javaBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-java_CLS"
task cppHeadersZip(type: Zip) {
destinationDirectory = outputsFolder
archiveBaseName = zipBaseName
archiveClassifier = "headers"
from(licenseFile) {
into '/'
}
from(photonVersionFile) {
into '/'
}
from('src/main/native/include/') {
into '/'
}
}
task cppSourcesZip(type: Zip) {
destinationDirectory = outputsFolder
archiveBaseName = zipBaseName
archiveClassifier = "sources"
from(licenseFile) {
into '/'
}
from('src/main/native/cpp') {
into '/'
}
}
build.dependsOn cppHeadersZip
addTaskToCopyAllOutputs(cppHeadersZip)
build.dependsOn cppSourcesZip
addTaskToCopyAllOutputs(cppSourcesZip)
task sourcesJar(type: Jar, dependsOn: classes) {
archiveClassifier = 'sources'
from sourceSets.main.allSource
}
task javadocJar(type: Jar, dependsOn: javadoc) {
archiveClassifier = 'javadoc'
from javadoc.destinationDir
}
task outputJar(type: Jar, dependsOn: classes) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
from sourceSets.main.output
}
task outputSourcesJar(type: Jar, dependsOn: classes) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
archiveClassifier = 'sources'
from sourceSets.main.allSource
}
task outputJavadocJar(type: Jar, dependsOn: javadoc) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
archiveClassifier = 'javadoc'
from javadoc.destinationDir
}
def vendorJson = artifacts.add('archives', file("$photonlibFileOutput"))
artifacts {
archives sourcesJar
archives javadocJar
archives outputJar
archives outputSourcesJar
archives outputJavadocJar
}
addTaskToCopyAllOutputs(outputSourcesJar)
addTaskToCopyAllOutputs(outputJavadocJar)
addTaskToCopyAllOutputs(outputJar)
build.dependsOn outputSourcesJar
build.dependsOn outputJavadocJar
build.dependsOn outputJar
libraryBuild.dependsOn build
def releasesRepoUrl = "$buildDir/repos/releases"
publishing {
repositories {
maven {
url = releasesRepoUrl
}
maven {
url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal'))
credentials {
username 'ghactions'
password System.getenv("ARTIFACTORY_API_KEY")
}
}
}
}
tasks.withType(PublishToMavenRepository) {
doFirst {
println("Publishing to " + repository.url)
}
}
task cleanReleaseRepo(type: Delete) {
delete releasesRepoUrl
}
tasks.matching {it != cleanReleaseRepo}.all {it.dependsOn cleanReleaseRepo}
model {
publishing {
def taskList = createComponentZipTasks($.components, ['Photon'], zipBaseName, Zip, project, includeStandardZipFormat)
publications {
cpp(MavenPublication) {
taskList.each {
artifact it
}
artifact cppHeadersZip
artifact cppSourcesZip
artifactId = "${baseArtifactId}-cpp"
groupId artifactGroupId
version pubVersion
}
java(MavenPublication) {
artifact jar
artifact sourcesJar
artifact javadocJar
artifactId = "${baseArtifactId}-java"
groupId artifactGroupId
version pubVersion
}
vendorjson(MavenPublication) {
artifact vendorJson
artifactId = "${baseArtifactId}-json"
groupId = artifactGroupId
version "1.0"
}
}
}
}
tasks.withType(PublishToMavenRepository) {
it.mustRunAfter generateVendorJson
}
publishToMavenLocal.dependsOn libraryBuild
publish.dependsOn libraryBuild

23
photon-lib/src/generate/photonlib.json.in
View File

@@ -13,9 +13,24 @@
"cppDependencies": [
{
"groupId": "org.photonvision",
"artifactId": "PhotonLib-cpp",
"artifactId": "photonlib-cpp",
"version": "${photon_version}",
"libName": "Photon",
"libName": "photonlib",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxathena",
"linuxx86-64",
"osxuniversal"
]
},
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
"version": "${photon_version}",
"libName": "photontargeting",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
@@ -30,12 +45,12 @@
"javaDependencies": [
{
"groupId": "org.photonvision",
"artifactId": "PhotonLib-java",
"artifactId": "photonlib-java",
"version": "${photon_version}"
},
{
"groupId": "org.photonvision",
"artifactId": "PhotonTargeting-java",
"artifactId": "photontargeting-java",
"version": "${photon_version}"
}
]

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

@@ -22,16 +22,16 @@
* SOFTWARE.
*/
#include "photonlib/PhotonCamera.h"
#include "photon/PhotonCamera.h"
#include <frc/Errors.h>
#include <frc/Timer.h>
#include <opencv2/core/mat.hpp>
#include "PhotonVersion.h"
#include "photonlib/Packet.h"
#include "photon/dataflow/structures/Packet.h"
namespace photonlib {
namespace photon {
constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
@@ -91,7 +91,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
const auto value = rawBytesEntry.Get();
if (!value.size()) return result;
photonlib::Packet packet{value};
photon::Packet packet{value};
packet >> result;
@@ -192,4 +192,4 @@ void PhotonCamera::VerifyVersion() {
}
}
} // namespace photonlib
} // namespace photon

26
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp → photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -22,7 +22,7 @@
* SOFTWARE.
*/
#include "photonlib/PhotonPoseEstimator.h"
#include "photon/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
@@ -44,11 +44,11 @@
#include <units/math.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/PhotonCamera.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photonlib {
namespace photon {
namespace detail {
cv::Point3d ToPoint3d(const frc::Translation3d& translation);
@@ -360,14 +360,14 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
if (result.MultiTagResult().result.isValid) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
return Update(result, std::nullopt, std::nullopt,
@@ -425,9 +425,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
const Pose3d pose = detail::ToPose3d(tvec, rvec);
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(),
MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
@@ -476,4 +476,4 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
}
} // namespace photonlib
} // namespace photon

113
photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp
View File

@@ -1,113 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photonlib/MultiTargetPNPResult.h"
namespace photonlib {
Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) {
packet << target.result;
size_t i;
for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
if (i < target.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(target.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) {
packet >> target.result;
target.fiducialIdsUsed.clear();
for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
target.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResults const& result) {
packet << result.isValid << result.best << result.alt
<< result.bestReprojectionErr << result.altReprojectionErr
<< result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResults& result) {
packet >> result.isValid >> result.best >> result.alt >>
result.bestReprojectionErr >> result.altReprojectionErr >>
result.ambiguity;
return packet;
}
} // namespace photonlib

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

@@ -1,71 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photonlib/PhotonPipelineResult.h"
namespace photonlib {
PhotonPipelineResult::PhotonPipelineResult(
units::second_t latency, std::span<const PhotonTrackedTarget> targets)
: latency(latency),
targets(targets.data(), targets.data() + targets.size()) {}
bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
return latency == other.latency && targets == other.targets;
}
bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
return !operator==(other);
}
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.latency.value() * 1000 << result.m_pnpResults
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
for (auto& target : result.targets) packet << target;
// Return the packet
return packet;
}
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
double latencyMillis = 0;
int8_t targetCount = 0;
packet >> latencyMillis >> result.m_pnpResults >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();
// Decode the information of each target.
for (int i = 0; i < targetCount; ++i) {
PhotonTrackedTarget target;
packet >> target;
result.targets.push_back(target);
}
return packet;
}
} // namespace photonlib

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

@@ -39,13 +39,17 @@
#include <units/time.h>
#include <wpi/deprecated.h>
#include "photonlib/PhotonPipelineResult.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace cv {
class Mat;
} // namespace cv
namespace photonlib {
namespace photon {
enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
@@ -207,4 +211,4 @@ class PhotonCamera {
void VerifyVersion();
};
} // namespace photonlib
} // namespace photon

12
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h → photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -30,14 +30,18 @@
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace cv {
class Mat;
} // namespace cv
namespace photonlib {
namespace photon {
enum PoseStrategy {
LOWEST_AMBIGUITY = 0,
CLOSEST_TO_CAMERA_HEIGHT,
@@ -296,4 +300,4 @@ class PhotonPoseEstimator {
PhotonPipelineResult result);
};
} // namespace photonlib
} // namespace photon

10
photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h → photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h
View File

@@ -26,9 +26,13 @@
#include <cmath>
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photonlib {
namespace photon {
namespace PhotonTargetSortMode {
@@ -82,4 +86,4 @@ struct CenterMost {
}
};
} // namespace PhotonTargetSortMode
} // namespace photonlib
} // namespace photon

10
photon-lib/src/main/native/include/photonlib/PhotonUtils.h → photon-lib/src/main/native/include/photon/PhotonUtils.h
View File

@@ -32,7 +32,13 @@
#include <units/length.h>
#include <units/math.h>
namespace photonlib {
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class PhotonUtils {
public:
/**
@@ -177,4 +183,4 @@ class PhotonUtils {
return fieldToTarget.TransformBy(targetToCamera);
}
};
} // namespace photonlib
} // namespace photon

13
photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h → photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
View File

@@ -32,10 +32,15 @@
#include <networktables/NetworkTableInstance.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonTargetSortMode.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonTargetSortMode.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photonlib {
namespace photon {
class SimPhotonCamera : public PhotonCamera {
public:
SimPhotonCamera(nt::NetworkTableInstance instance,
@@ -129,4 +134,4 @@ class SimPhotonCamera : public PhotonCamera {
nt::NetworkTableEntry versionEntry;
nt::RawPublisher rawBytesPublisher;
};
} // namespace photonlib
} // namespace photon

9
photon-lib/src/main/native/include/photonlib/SimVisionSystem.h → photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h
View File

@@ -34,8 +34,13 @@
#include "SimPhotonCamera.h"
#include "SimVisionTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photonlib {
namespace photon {
class SimVisionSystem {
public:
SimPhotonCamera cam;
@@ -218,4 +223,4 @@ class SimVisionSystem {
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
};
} // namespace photonlib
} // namespace photon

10
photon-lib/src/main/native/include/photonlib/SimVisionTarget.h → photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h
View File

@@ -27,7 +27,13 @@
#include <frc/geometry/Pose3d.h>
#include <units/area.h>
namespace photonlib {
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimVisionTarget {
public:
SimVisionTarget() = default;
@@ -56,4 +62,4 @@ class SimVisionTarget {
units::square_meter_t targetArea;
int targetId;
};
} // namespace photonlib
} // namespace photon

61
photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h
View File

@@ -1,61 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
namespace photonlib {
class PNPResults {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isValid;
frc::Transform3d best;
double bestReprojectionErr;
frc::Transform3d alt;
double altReprojectionErr;
double ambiguity;
friend Packet& operator<<(Packet& packet, const PNPResults& result);
friend Packet& operator>>(Packet& packet, PNPResults& result);
};
class MultiTargetPnpResult {
public:
PNPResults result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
};
} // namespace photonlib

106
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -35,10 +35,13 @@
#include <wpi/SmallVector.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonPoseEstimator.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -54,24 +57,24 @@ static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
@@ -83,8 +86,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(11));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -104,29 +107,29 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photonlib::PhotonCamera, frc::Transform3d>> cameras;
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -138,8 +141,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(17_s);
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
{{0_m, 0_m, 4_m}, {}});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -152,24 +155,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -181,9 +184,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(aprilTags,
photonlib::CLOSEST_TO_REFERENCE_POSE,
std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
@@ -197,24 +199,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
}
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -226,30 +228,30 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -272,24 +274,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
}
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -301,8 +303,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(15));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -314,24 +316,24 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
}
TEST(PhotonPoseEstimatorTest, PoseCache) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -341,8 +343,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
cameraOne.test = true;
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
// empty input, expect empty out
estimator.GetCamera()->testResult = {2_ms, {}};

2
photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp
View File

@@ -23,6 +23,6 @@
*/
#include "gtest/gtest.h"
#include "photonlib/PhotonUtils.h"
#include "photon/PhotonUtils.h"
TEST(PhotonUtilsTest, Include) {}

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

@@ -23,13 +23,13 @@
*/
#include "gtest/gtest.h"
#include "photonlib/PhotonUtils.h"
#include "photonlib/SimVisionSystem.h"
#include "photon/PhotonUtils.h"
#include "photon/simulation/SimVisionSystem.h"
class SimVisionSystemTest : public ::testing::Test {
void SetUp() override {
nt::NetworkTableInstance::GetDefault().StartServer();
photonlib::PhotonCamera::SetVersionCheckEnabled(false);
photon::PhotonCamera::SetVersionCheckEnabled(false);
}
void TearDown() override {}
@@ -44,7 +44,7 @@ class SimVisionSystemTestDistanceParamsTest
std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
TEST_F(SimVisionSystemTest, TestEmpty) {
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 320, 240, 0};
SUCCEED();
}
@@ -53,9 +53,9 @@ TEST_F(SimVisionSystemTest, TestVisibilityCupidShuffle) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 2_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3});
// To the right, to the right
frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{-70_deg}};
@@ -104,9 +104,9 @@ TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3});
frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
@@ -128,10 +128,10 @@ TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) {
// 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{
// photon::SimVisionSystem sys{
// "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0};
// sys.AddSimVisionTarget(
// photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
// photon::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
// frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}};
// sys.ProcessFrame(robotPose);
@@ -146,10 +146,10 @@ TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24});
photon::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24});
frc::Pose2d robotPose{{12_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
@@ -164,10 +164,9 @@ TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photonlib::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, 480, 1.0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});
photon::SimVisionSystem sys{"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640,
480, 1.0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
@@ -184,10 +183,9 @@ TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) {
frc::Rotation3d{0_deg, 0_deg,
(3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.ProcessFrame(robotPose);
@@ -195,7 +193,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) {
ASSERT_TRUE(results.HasTargets());
photonlib::PhotonTrackedTarget target = results.GetBestTarget();
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(GetParam().to<double>(), target.GetYaw(), 0.0001);
}
@@ -206,10 +204,9 @@ TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) {
frc::Rotation3d{0_deg, 0_deg,
(3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.MoveCamera(frc::Transform3d{frc::Translation3d{},
frc::Rotation3d{0_deg, GetParam(), 0_deg}});
@@ -219,7 +216,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) {
ASSERT_TRUE(results.HasTargets());
photonlib::PhotonTrackedTarget target = results.GetBestTarget();
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(GetParam().to<double>(), target.GetPitch(), 0.0001);
}
@@ -242,7 +239,7 @@ TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) {
frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}};
frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam},
frc::Rotation3d{0_deg, pitchParam, 0_deg}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
"wsyourdaygoingihopegoodhaveagreatrestofyourlife",
160.0_deg,
@@ -251,19 +248,18 @@ TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) {
640,
480,
0};
sys.AddSimVisionTarget(
photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
sys.ProcessFrame(robotPose);
auto results = sys.cam.GetLatestResult();
ASSERT_TRUE(results.HasTargets());
photonlib::PhotonTrackedTarget target = results.GetBestTarget();
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(target.GetYaw(), 0.0, 0.0001);
units::meter_t dist = photonlib::PhotonUtils::CalculateDistanceToTarget(
units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
robotToCamera.Z(), targetPose.Z(), pitchParam,
units::degree_t{target.GetPitch()});
ASSERT_NEAR(dist.to<double>(),
@@ -300,51 +296,51 @@ TEST_F(SimVisionSystemTest, TestMultipleTargets) {
const frc::Pose3d targetPoseR{
{15.98_m, -2_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photonlib::SimVisionSystem sys{
photon::SimVisionSystem sys{
"Test", 160.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 1});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 2});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 3});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 4});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 5});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 6});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 7});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 8});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 9});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 10});
sys.AddSimVisionTarget(photonlib::SimVisionTarget{
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 11});

66
photon-targeting/build.gradle
View File

@@ -1,38 +1,50 @@
plugins {
id "java"
id 'edu.wpi.first.WpilibTools' version '1.3.0'
}
apply from: "${rootDir}/shared/common.gradle"
ext {
nativeName = "photontargeting"
}
apply from: "${rootDir}/shared/setupBuild.gradle"
wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
def nativeConfigName = 'wpilibNatives'
def nativeConfig = configurations.create(nativeConfigName)
def nativeTasks = wpilibTools.createExtractionTasks {
configurationName = nativeConfigName
}
nativeTasks.addToSourceSetResources(sourceSets.main)
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
dependencies {
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("apriltag")
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("cameraserver")
implementation wpilibTools.deps.wpilibJava("cscore")
implementation wpilibTools.deps.wpilibJava("wpinet")
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation wpilibTools.deps.wpilibJava("apriltag")
implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get()
implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get()
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()
implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
}
tasks.withType(JavaCompile) {
options.encoding = 'UTF-8'
}
tasks.withType(Javadoc) {
options.encoding = 'UTF-8'
}
java {
withJavadocJar()
withSourcesJar()
}
test {
useJUnitPlatform()
}
apply from: "publish.gradle"

32
photon-targeting/publish.gradle
View File

@@ -1,32 +0,0 @@
apply plugin: 'maven-publish'
def artifactGroupId = 'org.photonvision'
def baseArtifactId = 'PhotonTargeting'
publishing {
repositories {
maven {
url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal'))
credentials {
username 'ghactions'
password System.getenv("ARTIFACTORY_API_KEY")
}
}
}
publications {
mavenJava(MavenPublication) {
groupId = artifactGroupId
artifactId = "${baseArtifactId}-java"
version = pubVersion
from components.java
}
}
}
tasks.withType(PublishToMavenRepository) {
doFirst {
println("Publishing PhotonTargeting to " + repository.url)
}
}

7
photon-targeting/settings.gradle
View File

@@ -1 +1,6 @@
rootProject.name = 'photon-targeting'
pluginManagement {
repositories {
mavenLocal()
gradlePluginPortal()
}
}

56
photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp Normal file
View File

@@ -0,0 +1,56 @@
/*
* 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/>.
*/
#include "photon/targeting/MultiTargetPNPResult.h"
namespace photon {
bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const {
return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
}
Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) {
packet << result.result;
size_t i;
for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
if (i < result.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(result.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) {
packet >> result.result;
result.fiducialIdsUsed.clear();
for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
result.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
} // namespace photon

77
photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp Normal file
View File

@@ -0,0 +1,77 @@
/*
* 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/>.
*/
#include "photon/targeting/PNPResult.h"
namespace photon {
bool PNPResult::operator==(const PNPResult& other) const {
return other.isPresent == isPresent && other.best == best &&
other.bestReprojErr == bestReprojErr && other.alt == alt &&
other.altReprojErr == altReprojErr && other.ambiguity == ambiguity;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResult const& result) {
packet << result.isPresent << result.best << result.alt
<< result.bestReprojErr << result.altReprojErr << result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResult& result) {
packet >> result.isPresent >> result.best >> result.alt >>
result.bestReprojErr >> result.altReprojErr >> result.ambiguity;
return packet;
}
} // namespace photon

67
photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp Normal file
View File

@@ -0,0 +1,67 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "photon/targeting/PhotonPipelineResult.h"
namespace photon {
PhotonPipelineResult::PhotonPipelineResult(
units::second_t latency, std::span<const PhotonTrackedTarget> targets)
: latency(latency),
targets(targets.data(), targets.data() + targets.size()) {}
PhotonPipelineResult::PhotonPipelineResult(
units::second_t latency, std::span<const PhotonTrackedTarget> targets,
MultiTargetPNPResult multitagResult)
: latency(latency),
targets(targets.data(), targets.data() + targets.size()),
multitagResult(multitagResult) {}
bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
return latency == other.latency && targets == other.targets &&
multitagResult == other.multitagResult;
}
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.latency.value() * 1000 << result.multitagResult
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
for (auto& target : result.targets) packet << target;
// Return the packet
return packet;
}
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
double latencyMillis = 0;
int8_t targetCount = 0;
packet >> latencyMillis >> result.multitagResult >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();
// Decode the information of each target.
for (int i = 0; i < targetCount; ++i) {
PhotonTrackedTarget target;
packet >> target;
result.targets.push_back(target);
}
return packet;
}
} // namespace photon

40
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp → photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp
View File

@@ -1,28 +1,21 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* 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:
* 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.
*
* 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.
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include <iostream>
#include <utility>
@@ -32,7 +25,7 @@
static constexpr const uint8_t MAX_CORNERS = 8;
namespace photonlib {
namespace photon {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id,
@@ -57,10 +50,6 @@ bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
other.minAreaRectCorners == minAreaRectCorners;
}
bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
return !operator==(other);
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
packet << target.yaw << target.pitch << target.area << target.skew
<< target.fiducialId
@@ -143,5 +132,4 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
return packet;
}
} // namespace photonlib
} // namespace photon

34
photon-lib/src/main/native/include/photonlib/Packet.h → photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* 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:
* 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.
*
* 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.
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
@@ -30,7 +23,7 @@
#include <wpi/Endian.h>
namespace photonlib {
namespace photon {
/**
* A packet that holds byte-packed data to be sent over NetworkTables.
@@ -126,5 +119,4 @@ class Packet {
size_t readPos = 0;
size_t writePos = 0;
};
} // namespace photonlib
} // namespace photon

37
photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h Normal file
View File

@@ -0,0 +1,37 @@
/*
* 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/>.
*/
#pragma once
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "PNPResult.h"
#include "photon/dataflow/structures/Packet.h"
namespace photon {
class MultiTargetPNPResult {
public:
PNPResult result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
bool operator==(const MultiTargetPNPResult& other) const;
friend Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPNPResult& result);
};
} // namespace photon

45
photon-targeting/src/main/native/include/photon/targeting/PNPResult.h Normal file
View File

@@ -0,0 +1,45 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
#include <frc/geometry/Transform3d.h>
#include "photon/dataflow/structures/Packet.h"
namespace photon {
class PNPResult {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isPresent;
frc::Transform3d best;
double bestReprojErr;
frc::Transform3d alt;
double altReprojErr;
double ambiguity;
bool operator==(const PNPResult& other) const;
friend Packet& operator<<(Packet& packet, const PNPResult& target);
friend Packet& operator>>(Packet& packet, PNPResult& target);
};
} // namespace photon

65
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h → photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* 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:
* 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.
*
* 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.
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
@@ -31,18 +24,18 @@
#include <units/time.h>
#include <wpi/SmallVector.h>
#include "photonlib/MultiTargetPNPResult.h"
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "MultiTargetPNPResult.h"
#include "PhotonTrackedTarget.h"
#include "photon/dataflow/structures/Packet.h"
namespace photonlib {
namespace photon {
/**
* Represents a pipeline result from a PhotonCamera.
*/
class PhotonPipelineResult {
public:
/**
* Constructs an empty pipeline result.
* Constructs an empty pipeline result
*/
PhotonPipelineResult() = default;
@@ -54,10 +47,20 @@ class PhotonPipelineResult {
PhotonPipelineResult(units::second_t latency,
std::span<const PhotonTrackedTarget> targets);
/**
* Constructs a pipeline result.
* @param latency The latency in the pipeline.
* @param targets The list of targets identified by the pipeline.
* @param multitagResult The multitarget result
*/
PhotonPipelineResult(units::second_t latency,
std::span<const PhotonTrackedTarget> targets,
MultiTargetPNPResult multitagResult);
/**
* Returns the best target in this pipeline result. If there are no targets,
* this method will return an empty target with all values set to zero. The
* best target is determined by the target sort mode in the PhotonVision UI.
* this method will return null. The best target is determined by the target
* sort mode in the PhotonVision UI.
*
* @return The best target of the pipeline result.
*/
@@ -90,10 +93,10 @@ class PhotonPipelineResult {
/**
* Return the latest mulit-target result, as calculated on your coprocessor.
* Be sure to check getMultiTagResult().estimatedPose.isValid before using the
* pose estimate!
* Be sure to check getMultiTagResult().estimatedPose.isPresent before using
* the pose estimate!
*/
const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; }
/**
* Sets the timestamp in seconds
@@ -118,16 +121,14 @@ class PhotonPipelineResult {
}
bool operator==(const PhotonPipelineResult& other) const;
bool operator!=(const PhotonPipelineResult& other) const;
friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
private:
units::second_t latency = 0_s;
units::second_t timestamp = -1_s;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
MultiTargetPnpResult m_pnpResults;
MultiTargetPNPResult multitagResult;
inline static bool HAS_WARNED = false;
};
} // namespace photonlib
} // namespace photon

47
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h → photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* 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:
* 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.
*
* 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.
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
@@ -32,9 +25,9 @@
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
#include "photon/dataflow/structures/Packet.h"
namespace photonlib {
namespace photon {
/**
* Represents a tracked target within a pipeline.
*/
@@ -53,13 +46,14 @@ class PhotonTrackedTarget {
* @param skew The skew of the target.
* @param pose The camera-relative pose of the target.
* @param alternatePose The alternate camera-relative pose of the target.
* @Param corners The corners of the bounding rectangle.
* @param minAreaRectCorners The corners of the bounding rectangle.
* @param detectedCorners All detected corners
*/
PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int fiducialID,
const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> corners,
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> detectedCorners);
/**
@@ -97,7 +91,8 @@ class PhotonTrackedTarget {
* down), in no particular order, of the minimum area bounding rectangle of
* this target
*/
wpi::SmallVector<std::pair<double, double>, 4> GetMinAreaRectCorners() const {
const wpi::SmallVector<std::pair<double, double>, 4>& GetMinAreaRectCorners()
const {
return minAreaRectCorners;
}
@@ -112,7 +107,7 @@ class PhotonTrackedTarget {
* V + Y | |
* 0 ----- 1
*/
std::vector<std::pair<double, double>> GetDetectedCorners() {
const std::vector<std::pair<double, double>>& GetDetectedCorners() const {
return detectedCorners;
}
@@ -141,12 +136,10 @@ class PhotonTrackedTarget {
}
bool operator==(const PhotonTrackedTarget& other) const;
bool operator!=(const PhotonTrackedTarget& other) const;
friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target);
friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target);
private:
double yaw = 0;
double pitch = 0;
double area = 0;
@@ -158,4 +151,4 @@ class PhotonTrackedTarget {
wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
std::vector<std::pair<double, double>> detectedCorners;
};
} // namespace photonlib
} // namespace photon

88
photon-lib/src/test/native/cpp/PacketTest.cpp → photon-targeting/src/test/native/cpp/PacketTest.cpp
View File

@@ -1,37 +1,53 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* 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:
* 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.
*
* 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.
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <units/angle.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
TEST(PacketTest, PNPResult) {
photon::PNPResult result;
photon::Packet p;
p << result;
photon::PNPResult b;
p >> b;
EXPECT_EQ(result, b);
}
TEST(PacketTest, MultiTargetPNPResult) {
photon::MultiTargetPNPResult result;
photon::Packet p;
p << result;
photon::MultiTargetPNPResult b;
p >> b;
EXPECT_EQ(result, b);
}
TEST(PacketTest, PhotonTrackedTarget) {
photonlib::PhotonTrackedTarget target{
photon::PhotonTrackedTarget target{
3.0,
4.0,
9.0,
@@ -45,31 +61,27 @@ TEST(PacketTest, PhotonTrackedTarget) {
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
photonlib::Packet p;
photon::Packet p;
p << target;
photonlib::PhotonTrackedTarget b;
photon::PhotonTrackedTarget b;
p >> b;
for (auto& c : p.GetData()) {
std::cout << static_cast<int>(c) << ",";
}
EXPECT_EQ(target, b);
}
TEST(PacketTest, PhotonPipelineResult) {
photonlib::PhotonPipelineResult result{1_s, {}};
photonlib::Packet p;
photon::PhotonPipelineResult result{1_s, {}};
photon::Packet p;
p << result;
photonlib::PhotonPipelineResult b;
photon::PhotonPipelineResult b;
p >> b;
EXPECT_EQ(result, b);
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 2> targets{
photon::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
@@ -82,7 +94,7 @@ TEST(PacketTest, PhotonPipelineResult) {
-1,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
@@ -97,11 +109,11 @@ TEST(PacketTest, PhotonPipelineResult) {
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
photonlib::PhotonPipelineResult result2{2_s, targets};
photonlib::Packet p2;
photon::PhotonPipelineResult result2{2_s, targets};
photon::Packet p2;
p2 << result2;
photonlib::PhotonPipelineResult b2;
photon::PhotonPipelineResult b2;
p2 >> b2;
EXPECT_EQ(result2, b2);

23
photon-targeting/src/test/native/cpp/main.cpp Normal file
View File

@@ -0,0 +1,23 @@
/*
* 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/>.
*/
#include "gtest/gtest.h"
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

25
photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
* 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/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/MultiTargetPNPResult.h"
// TODO
TEST(MultiTargetPNPResultTest, Equality) {}
// TODO
TEST(MultiTargetPNPResultTest, Inequality) {}

25
photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
* 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/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/PNPResult.h"
// TODO
TEST(PNPResultTest, Equality) {}
// TODO
TEST(PNPResultTest, Inequality) {}

25
photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
* 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/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/PhotonPipelineResult.h"
// TODO
TEST(PhotonPipelineResultTest, Equality) {}
// TODO
TEST(PhotonPipelineResultTest, Inequality) {}

25
photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
* 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/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/PhotonTrackedTarget.h"
// TODO
TEST(PhotonTrackedTargetTest, Equality) {}
// TODO
TEST(PhotonTrackedTargetTest, Inequality) {}

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

@@ -24,7 +24,7 @@
#include "Robot.h"
#include <photonlib/PhotonUtils.h>
#include <photon/PhotonUtils.h>
void Robot::TeleopPeriodic() {
double forwardSpeed;
@@ -37,7 +37,7 @@ void Robot::TeleopPeriodic() {
if (result.HasTargets()) {
// First calculate range
units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{result.GetBestTarget().GetPitch()});

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

@@ -24,7 +24,7 @@
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photon/PhotonCamera.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -60,7 +60,7 @@ class Robot : public frc::TimedRobot {
frc::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D};
// Change this to match the name of your camera
photonlib::PhotonCamera camera{"photonvision"};
photon::PhotonCamera camera{"photonvision"};
frc::XboxController xboxController{0};

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

@@ -24,7 +24,7 @@
#include "Robot.h"
#include <photonlib/PhotonUtils.h>
#include <photon/PhotonUtils.h>
void Robot::TeleopPeriodic() {
double forwardSpeed = -xboxController.GetRightY();
@@ -33,7 +33,7 @@ void Robot::TeleopPeriodic() {
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
photonlib::PhotonPipelineResult result = camera.GetLatestResult();
photon::PhotonPipelineResult result = camera.GetLatestResult();
if (result.HasTargets()) {
// Rotation speed is the output of the PID controller

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

@@ -24,7 +24,7 @@
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photon/PhotonCamera.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -40,7 +40,7 @@ class Robot : public frc::TimedRobot {
private:
// Change this to match the name of your camera
photonlib::PhotonCamera camera{"photonvision"};
photon::PhotonCamera camera{"photonvision"};
// PID constants should be tuned per robot
frc::PIDController controller{.1, 0, 0};

12
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -24,8 +24,8 @@
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photonlib/PhotonPoseEstimator.h>
#include <photon/PhotonCamera.h>
#include <photon/PhotonPoseEstimator.h>
#include <utility>
@@ -34,12 +34,12 @@
class PhotonCameraWrapper {
public:
photonlib::PhotonPoseEstimator m_poseEstimator{
photon::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photonlib::MULTI_TAG_PNP_ON_RIO,
std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
photon::MULTI_TAG_PNP_ON_RIO, std::move(photon::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};
inline std::optional<photonlib::EstimatedRobotPose> Update(
inline std::optional<photon::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {
m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
return m_poseEstimator.Update();

2
photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h
View File

@@ -24,7 +24,7 @@
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photon/PhotonCamera.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>

6
photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp
View File

@@ -24,7 +24,7 @@
#include "Robot.h"
#include <photonlib/PhotonUtils.h>
#include <photon/PhotonUtils.h>
void Robot::TeleopPeriodic() {
double forwardSpeed;
@@ -33,11 +33,11 @@ void Robot::TeleopPeriodic() {
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
photonlib::PhotonPipelineResult result = camera.GetLatestResult();
photon::PhotonPipelineResult result = camera.GetLatestResult();
if (result.HasTargets()) {
// First calculate range
units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{result.GetBestTarget().GetPitch()});

4
photonlib-cpp-examples/getinrange/src/main/include/Robot.h
View File

@@ -24,7 +24,7 @@
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photon/PhotonCamera.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -56,7 +56,7 @@ class Robot : public frc::TimedRobot {
frc::PIDController controller{P_GAIN, 0.0, D_GAIN};
// Change this to match the name of your camera
photonlib::PhotonCamera camera{"photonvision"};
photon::PhotonCamera camera{"photonvision"};
frc::XboxController xboxController{0};

2
shared/PhotonVersion.h.in
View File

@@ -23,7 +23,7 @@
* regenerated any time the publish task is run, or when this file is deleted.
*/
namespace photonlib {
namespace photon {
namespace PhotonVersion {
const std::string versionString = "${version}";
const std::string buildDate = "${date}";

3
shared/common.gradle
View File

@@ -37,6 +37,7 @@ dependencies {
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation wpilibTools.deps.wpilibJava("apriltag")
implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
// Jackson
@@ -84,7 +85,7 @@ tasks.register('generateJavaDocs', Javadoc) {
}
jacoco {
toolVersion = "0.8.9"
toolVersion = "0.8.10"
reportsDirectory = layout.buildDirectory.dir('customJacocoReportDir')
}

16
shared/config.gradle
View File

@@ -20,6 +20,8 @@ nativeUtils.wpi.configureDependencies {
nativeUtils.wpi.addWarnings()
nativeUtils.wpi.addWarningsAsErrors()
nativeUtils.setSinglePrintPerPlatform()
// Enable builds for all platforms.
model {
components {
@@ -34,6 +36,18 @@ model {
}
}
task copyAllOutputs(type: Copy) {
def outputsFolder = file("$project.buildDir/outputs")
destinationDir outputsFolder
}
ext.addTaskToCopyAllOutputs = { task ->
copyAllOutputs.dependsOn task
copyAllOutputs.inputs.file task.archiveFile
copyAllOutputs.from task.archiveFile
}
// Add debug path to binaries.
ext.appendDebugPathToBinaries = { binaries ->
binaries.withType(StaticLibraryBinarySpec) {
@@ -69,6 +83,8 @@ ext.appendDebugPathToBinaries = { binaries ->
}
}
def licenseFile = file("$rootDir/LICENCE")
// Create ZIP tasks for each component.
ext.createComponentZipTasks = { components, names, base, type, project, func ->
def stringNames = names.collect { it.toString() }

115
shared/javacommon.gradle Normal file
View File

@@ -0,0 +1,115 @@
apply plugin: 'maven-publish'
apply plugin: 'java-library'
apply plugin: 'jacoco'
java {
sourceCompatibility = JavaVersion.VERSION_11
targetCompatibility = JavaVersion.VERSION_11
}
def baseArtifactId = nativeName
def artifactGroupId = 'org.photonvision'
def javaBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-java_CLS"
def outputsFolder = file("$buildDir/outputs")
javadoc {
options.encoding = 'UTF-8'
}
task sourcesJar(type: Jar, dependsOn: classes) {
archiveClassifier = 'sources'
from sourceSets.main.allSource
}
task javadocJar(type: Jar, dependsOn: javadoc) {
archiveClassifier = 'javadoc'
from javadoc.destinationDir
}
task outputJar(type: Jar, dependsOn: classes) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
from sourceSets.main.output
}
task outputSourcesJar(type: Jar, dependsOn: classes) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
archiveClassifier = 'sources'
from sourceSets.main.allSource
}
task outputJavadocJar(type: Jar, dependsOn: javadoc) {
archiveBaseName = javaBaseName
destinationDirectory = outputsFolder
archiveClassifier = 'javadoc'
from javadoc.destinationDir
}
artifacts {
archives sourcesJar
archives javadocJar
archives outputJar
archives outputSourcesJar
archives outputJavadocJar
}
addTaskToCopyAllOutputs(outputSourcesJar)
addTaskToCopyAllOutputs(outputJavadocJar)
addTaskToCopyAllOutputs(outputJar)
build.dependsOn outputSourcesJar
build.dependsOn outputJavadocJar
build.dependsOn outputJar
publishing {
publications {
java(MavenPublication) {
artifact jar
artifact sourcesJar
artifact javadocJar
artifactId = "${baseArtifactId}-java"
groupId artifactGroupId
version pubVersion
}
}
repositories {
maven {
url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal'))
credentials {
username 'ghactions'
password System.getenv("ARTIFACTORY_API_KEY")
}
}
}
}
test {
useJUnitPlatform()
systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
testLogging {
events "failed"
exceptionFormat "full"
}
finalizedBy jacocoTestReport
}
dependencies {
testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0'
testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0'
testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0'
}
jacoco {
toolVersion = "0.8.10"
}
jacocoTestReport {
reports {
xml.required = true
html.required = true
}
}

81
shared/publish.gradle Normal file
View File

@@ -0,0 +1,81 @@
apply plugin: 'maven-publish'
def outputsFolder = file("$buildDir/outputs")
def baseArtifactId = nativeName
def artifactGroupId = 'org.photonvision'
def zipBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-cpp_CLS"
def licenseFile = file("$rootDir/LICENCE")
task cppSourcesZip(type: Zip) {
destinationDirectory = outputsFolder
archiveBaseName = zipBaseName
archiveClassifier = "sources"
from(licenseFile) {
into '/'
}
from('src/main/native/cpp') {
into '/'
}
}
task cppHeadersZip(type: Zip) {
destinationDirectory = outputsFolder
archiveBaseName = zipBaseName
archiveClassifier = "headers"
from(licenseFile) {
into '/'
}
ext.includeDirs = [
project.file('src/main/native/include')
]
ext.includeDirs.each {
from(it) {
into '/'
}
}
}
artifacts {
archives cppHeadersZip
archives cppSourcesZip
}
addTaskToCopyAllOutputs(cppSourcesZip)
addTaskToCopyAllOutputs(cppHeadersZip)
model {
publishing {
def taskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat)
publications {
cpp(MavenPublication) {
taskList.each {
artifact it
}
artifact cppHeadersZip
artifact cppSourcesZip
artifactId = "${baseArtifactId}-cpp"
groupId artifactGroupId
version pubVersion
}
}
repositories {
maven {
url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal'))
credentials {
username 'ghactions'
password System.getenv("ARTIFACTORY_API_KEY")
}
}
}
}
}

76
shared/setupBuild.gradle Normal file
View File

@@ -0,0 +1,76 @@
apply plugin: 'cpp'
apply plugin: 'google-test-test-suite'
apply plugin: 'edu.wpi.first.NativeUtils'
apply from: "${rootDir}/shared/config.gradle"
apply from: "${rootDir}/shared/javacommon.gradle"
// Windows specific functionality to export all symbols from a binary automatically
nativeUtils {
exportsConfigs {
"${nativeName}" {}
}
}
model {
components {
"${nativeName}"(NativeLibrarySpec) {
sources {
cpp {
source {
srcDirs 'src/main/native/cpp'
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/main/native/include'
include "**/*.h"
}
}
}
if(project.hasProperty('includePhotonTargeting')) {
binaries.all {
lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
}
}
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
testSuites {
"${nativeName}Test"(GoogleTestTestSuiteSpec) {
for(NativeComponentSpec c : $.components) {
if (c.name == nativeName) {
testing c
break
}
}
sources {
cpp {
source {
srcDirs 'src/test/native/cpp'
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/test/native/include', 'src/main/native/cpp'
}
}
}
if(project.hasProperty('includePhotonTargeting')) {
binaries.all {
lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
}
}
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
nativeUtils.useRequiredLibrary(it, "googletest_static")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
}
apply from: "${rootDir}/shared/publish.gradle"