mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-23 01:21:40 +00:00
401 lines
16 KiB
C++
401 lines
16 KiB
C++
|
/*
|
||
|
|
* 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 <networktables/NetworkTable.h>
|
||
|
|
#include <networktables/NetworkTableEntry.h>
|
||
|
|
#include <networktables/NetworkTableInstance.h>
|
||
|
|
#include <units/angle.h>
|
||
|
|
#include <units/length.h>
|
||
|
|
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
#include "photonlib/PhotonCamera.h"
|
||
|
|
#include "photonlib/PhotonUtils.h"
|
||
|
|
#include "photonlib/SimVisionSystem.h"
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testEmpty) {
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
320, 240, 0.0);
|
||
|
|
|
||
|
|
for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
|
||
|
|
sysUnderTest.ProcessFrame(frc::Pose2d());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
class SimVisionSystemTestDistParam : public testing::TestWithParam<double> {};
|
||
|
|
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestDistParamInst,
|
||
|
|
SimVisionSystemTestDistParam,
|
||
|
|
testing::Values(5, 10, 15, 20, 25, 30));
|
||
|
|
|
||
|
|
TEST_P(SimVisionSystemTestDistParam, testDistanceAligned) {
|
||
|
|
double dist = GetParam();
|
||
|
|
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
320, 240, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 0.0_m, 1.0_m, 1.0_m));
|
||
|
|
|
||
|
|
auto robotPose = frc::Pose2d(
|
||
|
|
frc::Translation2d(units::meter_t(35.0 - dist), 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
ASSERT_EQ(result.GetBestTarget()
|
||
|
|
.GetCameraRelativePose()
|
||
|
|
.Translation()
|
||
|
|
.Norm()
|
||
|
|
.to<double>(),
|
||
|
|
dist);
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testVisibilityCupidShuffle) {
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
320, 240, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 0.0_m, 3.0_m, 3.0_m));
|
||
|
|
|
||
|
|
// To the right, to the right
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-70.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
|
||
|
|
// To the right, to the right
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-95.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
|
||
|
|
// To the left, to the left
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(90.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
|
||
|
|
// To the left, to the left
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(65.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
|
||
|
|
// now kick, now kick
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
// now kick, now kick
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(-5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
// now walk it by yourself
|
||
|
|
robotPose = frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m),
|
||
|
|
frc::Rotation2d(-179.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
|
||
|
|
// now walk it by yourself
|
||
|
|
sysUnderTest.MoveCamera(
|
||
|
|
frc::Transform2d(frc::Translation2d(), frc::Rotation2d(180_deg)), 0.0_m,
|
||
|
|
1.0_deg);
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testNotVisibleVert1) {
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
640, 480, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 1.0_m, 3.0_m, 2.0_m));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
sysUnderTest.MoveCamera(
|
||
|
|
frc::Transform2d(frc::Translation2d(), frc::Rotation2d(0_deg)), 5000.0_m,
|
||
|
|
1.0_deg);
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testNotVisibleVert2) {
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 45.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
1234, 1234, 0.5);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 3.0_m, 0.5_m, 0.5_m));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testNotVisibleTgtSize) {
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
640, 480, 20.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testNotVisibleTooFarForLEDs) {
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 10.0_m,
|
||
|
|
640, 480, 1.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(28.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
EXPECT_FALSE(result.HasTargets());
|
||
|
|
}
|
||
|
|
|
||
|
|
class SimVisionSystemTestYawParam : public testing::TestWithParam<double> {};
|
||
|
|
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestYawParamInst,
|
||
|
|
SimVisionSystemTestYawParam,
|
||
|
|
testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23));
|
||
|
|
TEST_P(SimVisionSystemTestYawParam, testYawAngles) {
|
||
|
|
double testYaw = GetParam(); // Nope, Chuck testYaw
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
640, 480, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
|
||
|
|
|
||
|
|
auto robotPose = frc::Pose2d(frc::Translation2d(32_m, 0.0_m),
|
||
|
|
frc::Rotation2d(units::degree_t(testYaw)));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
auto tgt = result.GetBestTarget();
|
||
|
|
EXPECT_DOUBLE_EQ(tgt.GetYaw(), testYaw);
|
||
|
|
}
|
||
|
|
|
||
|
|
class SimVisionSystemTestCameraPitchParam
|
||
|
|
: public testing::TestWithParam<double> {};
|
||
|
|
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestCameraPitchParamInst,
|
||
|
|
SimVisionSystemTestCameraPitchParam,
|
||
|
|
testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23,
|
||
|
|
20.21, -19.999));
|
||
|
|
TEST_P(SimVisionSystemTestCameraPitchParam, testCameraPitch) {
|
||
|
|
double testPitch = GetParam();
|
||
|
|
auto targetPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(0.0_deg));
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||
|
|
640, 480, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
|
||
|
|
|
||
|
|
sysUnderTest.MoveCamera(
|
||
|
|
frc::Transform2d(frc::Translation2d(), frc::Rotation2d()), 0.0_m,
|
||
|
|
units::degree_t(testPitch));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
auto tgt = result.GetBestTarget();
|
||
|
|
// If the camera is pitched down by 10 degrees, the target should appear
|
||
|
|
// in the upper part of the image (ie, pitch positive). Therefor,
|
||
|
|
// pass/fail involves -1.0.
|
||
|
|
EXPECT_DOUBLE_EQ(tgt.GetPitch(), -1.0 * testPitch);
|
||
|
|
}
|
||
|
|
|
||
|
|
class SimVisionSystemTestDistCalcParam
|
||
|
|
: public testing::TestWithParam<std::tuple<double, double, double>> {};
|
||
|
|
INSTANTIATE_TEST_SUITE_P(
|
||
|
|
SimVisionSystemTestDistCalcParamInst, SimVisionSystemTestDistCalcParam,
|
||
|
|
testing::Values(std::tuple<double, double, double>(5, 35, 0),
|
||
|
|
std::tuple<double, double, double>(6, 35, 1),
|
||
|
|
std::tuple<double, double, double>(10, 35, 0),
|
||
|
|
std::tuple<double, double, double>(15, 35, 2),
|
||
|
|
std::tuple<double, double, double>(19.95, 35, 0),
|
||
|
|
std::tuple<double, double, double>(20, 35, 0),
|
||
|
|
std::tuple<double, double, double>(5, 42, 1),
|
||
|
|
std::tuple<double, double, double>(6, 42, 0),
|
||
|
|
std::tuple<double, double, double>(10, 42, 2),
|
||
|
|
std::tuple<double, double, double>(15, 42, 0.5),
|
||
|
|
std::tuple<double, double, double>(19.42, 35, 0),
|
||
|
|
std::tuple<double, double, double>(20, 42, 0),
|
||
|
|
std::tuple<double, double, double>(5, 55, 2),
|
||
|
|
std::tuple<double, double, double>(6, 55, 0),
|
||
|
|
std::tuple<double, double, double>(10, 54, 2.2),
|
||
|
|
std::tuple<double, double, double>(15, 53, 0),
|
||
|
|
std::tuple<double, double, double>(19.52, 35, 1.1),
|
||
|
|
std::tuple<double, double, double>(20, 51, 2.87),
|
||
|
|
std::tuple<double, double, double>(20, 55, 3)));
|
||
|
|
TEST_P(SimVisionSystemTestDistCalcParam, testDistanceCalc) {
|
||
|
|
std::tuple<double, double, double> testArgs = GetParam();
|
||
|
|
double testDist = std::get<0>(testArgs);
|
||
|
|
double testPitch = std::get<1>(testArgs);
|
||
|
|
double testHeight = std::get<2>(testArgs);
|
||
|
|
|
||
|
|
auto targetPose = frc::Pose2d(frc::Translation2d(35_m, 0_m),
|
||
|
|
frc::Rotation2d(units::radian_t(3.14159 / 42)));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(units::meter_t(35 - testDist), 0.0_m),
|
||
|
|
frc::Rotation2d(0.0_deg));
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest(
|
||
|
|
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
|
||
|
|
"wsyourdaygoingihopegoodhaveagreatrestofyourlife",
|
||
|
|
160.0_deg, units::degree_t(testPitch), frc::Transform2d(),
|
||
|
|
units::meter_t(testHeight), 99999.0_m, 640, 480, 0.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(photonlib::SimVisionTarget(
|
||
|
|
targetPose, units::meter_t(testDist), 0.5_m, 0.5_m));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
auto tgt = result.GetBestTarget();
|
||
|
|
EXPECT_DOUBLE_EQ(tgt.GetYaw(), 0.0);
|
||
|
|
units::meter_t distMeas = photonlib::PhotonUtils::CalculateDistanceToTarget(
|
||
|
|
units::meter_t(testHeight), units::meter_t(testDist),
|
||
|
|
units::degree_t(testPitch), units::degree_t(tgt.GetPitch()));
|
||
|
|
EXPECT_DOUBLE_EQ(distMeas.to<double>(), testDist);
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(SimVisionSystemTest, testMultipleTargets) {
|
||
|
|
auto targetPoseL =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 2_m), frc::Rotation2d());
|
||
|
|
auto targetPoseC =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||
|
|
auto targetPoseR =
|
||
|
|
frc::Pose2d(frc::Translation2d(35_m, -2_m), frc::Rotation2d());
|
||
|
|
|
||
|
|
photonlib::SimVisionSystem sysUnderTest("test", 160.0_deg, 0.0_deg,
|
||
|
|
frc::Transform2d(), 5.0_m, 99999.0_m,
|
||
|
|
640, 480, 20.0);
|
||
|
|
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseL, 0.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseC, 1.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseR, 2.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseL, 3.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseC, 4.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseR, 5.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseL, 6.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseC, 7.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseL, 8.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseR, 9.0_m, 0.25_m, 0.1_m));
|
||
|
|
sysUnderTest.AddSimVisionTarget(
|
||
|
|
photonlib::SimVisionTarget(targetPoseL, 10.0_m, 0.25_m, 0.1_m));
|
||
|
|
|
||
|
|
auto robotPose =
|
||
|
|
frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||
|
|
|
||
|
|
sysUnderTest.ProcessFrame(robotPose);
|
||
|
|
auto result = sysUnderTest.cam.GetLatestResult();
|
||
|
|
ASSERT_TRUE(result.HasTargets());
|
||
|
|
|
||
|
|
auto tgtList = result.GetTargets();
|
||
|
|
EXPECT_EQ(11ul, tgtList.size());
|
||
|
|
}
|