mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)
* 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
@@ -1,108 +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 <iostream>
|
||||
|
||||
#include <units/angle.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "photonlib/PhotonPipelineResult.h"
|
||||
#include "photonlib/PhotonTrackedTarget.h"
|
||||
|
||||
TEST(PacketTest, PhotonTrackedTarget) {
|
||||
photonlib::PhotonTrackedTarget target{
|
||||
3.0,
|
||||
4.0,
|
||||
9.0,
|
||||
-5.0,
|
||||
-1,
|
||||
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)),
|
||||
-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::Packet p;
|
||||
p << target;
|
||||
|
||||
photonlib::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;
|
||||
p << result;
|
||||
|
||||
photonlib::PhotonPipelineResult b;
|
||||
p >> b;
|
||||
|
||||
EXPECT_EQ(result, b);
|
||||
|
||||
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
|
||||
photonlib::PhotonTrackedTarget{
|
||||
3.0,
|
||||
-4.0,
|
||||
9.0,
|
||||
4.0,
|
||||
1,
|
||||
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)),
|
||||
-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{
|
||||
3.0,
|
||||
-4.0,
|
||||
9.1,
|
||||
6.7,
|
||||
-1,
|
||||
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)),
|
||||
-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::PhotonPipelineResult result2{2_s, targets};
|
||||
photonlib::Packet p2;
|
||||
p2 << result2;
|
||||
|
||||
photonlib::PhotonPipelineResult b2;
|
||||
p2 >> b2;
|
||||
|
||||
EXPECT_EQ(result2, b2);
|
||||
}
|
||||
@@ -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, {}};
|
||||
|
||||
@@ -23,6 +23,6 @@
|
||||
*/
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "photonlib/PhotonUtils.h"
|
||||
#include "photon/PhotonUtils.h"
|
||||
|
||||
TEST(PhotonUtilsTest, Include) {}
|
||||
|
||||
@@ -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});
|
||||
|
||||
Reference in New Issue
Block a user