Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-02 02:51:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Make 2027 build (#2422)

Browse Source 
This PR updates everything for 2027. This includes removing GradleRIO, simplifying our wpilib version defintion, updating APIs, updating to Java 21, and more.

Note that photonlibpy is failing because robotpy has not been fully updated yet. Examples are omitted because they need to be updated for our new PhotonPoseEstimator API and still need some changes from WPILIB. photonlib windows build is failing because we're waiting for some upstream changes. Finally, images are failing since they don't have Java 21 yet.
This commit is contained in:
Sam Freund
2026-04-11 12:14:42 -05:00
committed by samfreund
parent 4412df1516
commit 68fc1e7129
111 changed files with 630 additions and 578 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -33,6 +33,7 @@ import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.common.networktables.PacketSubscriber;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.timesync.TimeSyncSingleton;
import org.wpilib.driverstation.Alert;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.linalg.MatBuilder;
@@ -51,8 +52,6 @@ import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.PubSubOption;
import org.wpilib.networktables.StringSubscriber;
import org.wpilib.system.Timer;
import org.wpilib.util.Alert;
import org.wpilib.util.Alert.AlertType;
/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera implements AutoCloseable {
@@ -137,8 +136,8 @@ public class PhotonCamera implements AutoCloseable {
name = cameraName;
disconnectAlert =
new Alert(
PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning);
timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning);
PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", Alert.Level.MEDIUM);
timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", Alert.Level.MEDIUM);
rootPhotonTable = instance.getTable(kTableName);
this.cameraTable = rootPhotonTable.getSubTable(cameraName);
path = cameraTable.getPath();
@@ -149,7 +148,7 @@ public class PhotonCamera implements AutoCloseable {
PhotonPipelineResult.photonStruct.getTypeString(),
new byte[0],
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.SEND_ALL,
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
@@ -172,8 +171,7 @@ public class PhotonCamera implements AutoCloseable {
// Existing is enough to make this multisubscriber do its thing
topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
new MultiSubscriber(instance, new String[] {"/photonvision/"}, PubSubOption.TOPICS_ONLY);
InstanceCount++;
HAL.reportUsage("PhotonVision/PhotonCamera", InstanceCount, "");
@@ -576,7 +574,7 @@ public class PhotonCamera implements AutoCloseable {
// spotless:on
DriverStation.reportWarning(bfw, false);
var versionMismatchMessage =
String versionMismatchMessage =
"Photon version "
+ PhotonVersion.versionString
+ " (message definition version "

4
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -152,7 +152,7 @@ public class PhotonPoseEstimator {
this.fieldTags = fieldTags;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
InstanceCount++;
}
@@ -1140,7 +1140,7 @@ public class PhotonPoseEstimator {
double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity;
Pose3d estimatedPose = pair.getSecond();
transform = transform.plus(estimatedPose.getTranslation().times(weight));
rotation = rotation.plus(estimatedPose.getRotation().times(weight));
rotation = rotation.rotateBy(estimatedPose.getRotation().times(weight));
}
return Optional.of(

2
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -531,7 +531,7 @@ public class PhotonCameraSim implements AutoCloseable {
// Simulate confidence using sqrt-scaled area for a more realistic
// curve. Raw areaPercent/100 is tiny for most targets; sqrt scaling
// gives reasonable values even for small-but-visible objects.
conf = (float) MathUtil.clamp(Math.sqrt(areaPercent / 100.0) * 2.0, 0.0, 1.0);
conf = (float) Math.clamp(Math.sqrt(areaPercent / 100.0) * 2.0, 0.0, 1.0);
}
detectableTgts.add(

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

@@ -29,15 +29,16 @@
#include <string_view>
#include <vector>
#include <wpi/hal/UsageReporting.h>
#include <net/TimeSyncServer.h>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <wpi/hal/UsageReporting.hpp>
#include <wpi/system/Errors.hpp>
#include <wpi/system/RobotController.hpp>
#include <wpi/system/Timer.hpp>
#include <wpi/system/WPILibVersion.hpp>
#include <wpi/util/json.hpp>
#include <wpi/util/string.hpp>
#include "PhotonVersion.h"
#include "photon/dataflow/structures/Packet.h"
@@ -164,8 +165,8 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance,
disconnectAlert(PHOTON_ALERT_GROUP,
std::string{"PhotonCamera '"} + std::string{cameraName} +
"' is disconnected.",
wpi::Alert::AlertType::kWarning),
timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::AlertType::kWarning) {
wpi::Alert::Level::MEDIUM),
timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::Level::MEDIUM) {
verifyDependencies();
InstanceCount++;
HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, "");

16
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -33,8 +33,7 @@
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <wpi/deprecated.h>
#include <wpi/hal/UsageReporting.h>
#include <wpi/hal/UsageReporting.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
@@ -42,6 +41,7 @@
#include <wpi/units/angle.hpp>
#include <wpi/units/math.hpp>
#include <wpi/units/time.hpp>
#include <wpi/util/deprecated.hpp>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -74,8 +74,7 @@ PhotonPoseEstimator::PhotonPoseEstimator(
poseCacheTimestamp(-1_s),
headingBuffer(
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
InstanceCount++;
}
@@ -187,10 +186,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
break;
case CONSTRAINED_SOLVEPNP: {
using namespace frc;
if (!cameraMatrixData || !cameraDistCoeffs) {
WPILib_ReportError(
WPILIB_ReportError(
wpi::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
@@ -255,7 +252,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
WPILib_ReportError(wpi::warn::Warning,
WPILIB_ReportError(wpi::warn::Warning,
"Result timestamp was reported in the past!");
return false;
}
@@ -624,7 +621,7 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
pair : tempPoses) {
double const weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
rotation = rotation.RotateBy(pair.first.Rotation() * weight);
}
return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation),
@@ -661,7 +658,6 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
aprilTags, photon::kAprilTag36h11, headingFree,
wpi::math::Rotation2d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
headingScaleFactor);

20
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -51,7 +51,7 @@ PhotonCameraSim::PhotonCameraSim(
videoSimRaw =
wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
prop.GetResWidth(), prop.GetResHeight());
videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray);
videoSimRaw.SetPixelFormat(wpi::util::PixelFormat::kGray);
videoSimProcessed = wpi::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
prop.GetResHeight());
@@ -89,14 +89,16 @@ bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
return true;
}
std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
uint64_t now = wpi::util::Now();
uint64_t timestamp{};
int64_t now = wpi::nt::Now();
int64_t timestamp{};
bool hasTimestamp = false;
int iter = 0;
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
uint64_t frameTime = prop.EstSecUntilNextFrame()
.convert<wpi::units::microseconds>()
.to<uint64_t>();
hasTimestamp = true;
int64_t frameTime = prop.EstSecUntilNextFrame()
.convert<wpi::units::microseconds>()
.to<int64_t>();
nextNTEntryTime += frameTime;
if (iter++ > 50) {
@@ -106,8 +108,8 @@ std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
}
}
if (timestamp != 0) {
return timestamp;
if (hasTimestamp) {
return static_cast<uint64_t>(timestamp);
} else {
return std::nullopt;
}
@@ -363,7 +365,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::util::Now());
SubmitProcessedFrame(result, wpi::nt::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t ReceiveTimestamp) {

26
photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp
View File

@@ -28,7 +28,7 @@
#include <utility>
#include <vector>
#include <frc/Errors.h>
#include <wpi/system/Errors.hpp>
using namespace photon;
@@ -75,32 +75,32 @@ void SimCameraProperties::SetCalibration(
1_m,
wpi::math::Rotation3d{
0_rad, 0_rad,
(GetPixelYaw(0) +
wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}})
(GetPixelYaw(0) + wpi::math::Rotation2d{wpi::units::radian_t{
-std::numbers::pi / 2.0}})
.Radians()}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad, 0_rad,
(GetPixelYaw(width) +
wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}})
(GetPixelYaw(width) + wpi::math::Rotation2d{wpi::units::radian_t{
std::numbers::pi / 2.0}})
.Radians()}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad,
(GetPixelPitch(0) +
wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}})
(GetPixelPitch(0) + wpi::math::Rotation2d{wpi::units::radian_t{
std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad,
(GetPixelPitch(height) +
wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
wpi::math::Rotation3d{0_rad,
(GetPixelPitch(height) +
wpi::math::Rotation2d{wpi::units::radian_t{
-std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
};
viewplanes.clear();
for (size_t i = 0; i < p.size(); i++) {

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

@@ -28,6 +28,7 @@
#include <string>
#include <vector>
#include <wpi/driverstation/Alert.hpp>
#include <wpi/nt/BooleanTopic.hpp>
#include <wpi/nt/DoubleArrayTopic.hpp>
#include <wpi/nt/DoubleTopic.hpp>
@@ -38,7 +39,6 @@
#include <wpi/nt/RawTopic.hpp>
#include <wpi/nt/StringTopic.hpp>
#include <wpi/units/time.hpp>
#include <wpi/util/Alert.hpp>
#include "photon/targeting/PhotonPipelineResult.h"
@@ -204,7 +204,9 @@ class PhotonCamera {
*/
static void SetVersionCheckEnabled(bool enabled);
std::shared_ptr<wpi::nt::NetworkTable> GetCameraTable() const { return rootTable; }
std::shared_ptr<wpi::nt::NetworkTable> GetCameraTable() const {
return rootTable;
}
// For use in tests
bool test = false;

10
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -27,12 +27,12 @@
#include <optional>
#include <span>
#include <wpi/util/SmallVector.hpp>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/math/interpolation/TimeInterpolatableBuffer.hpp>
#include <wpi/util/SmallVector.hpp>
#include "photon/PhotonCamera.h"
#include "photon/targeting/PhotonPipelineResult.h"
@@ -94,7 +94,7 @@ class PhotonPoseEstimator {
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags,
wpi::math::Transform3d robotToCamera);
/**
@@ -212,9 +212,7 @@ class PhotonPoseEstimator {
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(wpi::math::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
@@ -417,8 +415,8 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, wpi::math::Pose3d seedPose,
bool headingFree, double headingScaleFactor);
photon::PhotonCamera::DistortionMatrix distCoeffs,
wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor);
private:
wpi::apriltag::AprilTagFieldLayout aprilTags;

7
photon-lib/src/main/native/include/photon/PhotonUtils.h
View File

@@ -50,10 +50,9 @@ class PhotonUtils {
* values up.
* @return The estimated distance to the target.
*/
static wpi::units::meter_t CalculateDistanceToTarget(wpi::units::meter_t cameraHeight,
wpi::units::meter_t targetHeight,
wpi::units::radian_t cameraPitch,
wpi::units::radian_t targetPitch) {
static wpi::units::meter_t CalculateDistanceToTarget(
wpi::units::meter_t cameraHeight, wpi::units::meter_t targetHeight,
wpi::units::radian_t cameraPitch, wpi::units::radian_t targetPitch) {
return (targetHeight - cameraHeight) /
wpi::units::math::tan(cameraPitch + targetPitch);
}

9
photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View File

@@ -88,7 +88,8 @@ class PhotonCameraSim {
* separate from this.
*/
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
double minTargetAreaPercent, wpi::units::meter_t maxSightRange);
double minTargetAreaPercent,
wpi::units::meter_t maxSightRange);
/**
* Returns the camera being simulated.
@@ -183,7 +184,9 @@ class PhotonCameraSim {
*
* @param rangeMeters The distance
*/
inline void SetMaxSightRange(wpi::units::meter_t range) { maxSightRange = range; }
inline void SetMaxSightRange(wpi::units::meter_t range) {
maxSightRange = range;
}
/**
* Sets whether the raw video stream simulation is enabled.
@@ -242,7 +245,7 @@ class PhotonCameraSim {
NTTopicSet ts{};
int64_t heartbeatCounter{0};
uint64_t nextNTEntryTime{wpi::util::Now()};
int64_t nextNTEntryTime{wpi::nt::Now()};
wpi::units::meter_t maxSightRange{std::numeric_limits<double>::max()};
static constexpr double kDefaultMinAreaPx{100};

8
photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
View File

@@ -290,8 +290,8 @@ class SimCameraProperties {
}
wpi::math::Rotation2d GetDiagFOV() const {
return wpi::math::Rotation2d{
wpi::units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
return wpi::math::Rotation2d{wpi::units::math::hypot(
GetHorizFOV().Radians(), GetVertFOV().Radians())};
}
/**
@@ -353,8 +353,8 @@ class SimCameraProperties {
* @return The latency estimate
*/
wpi::units::second_t EstLatency() {
return wpi::units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
0_s);
return wpi::units::math::max(
avgLatency + gaussian(generator) * latencyStdDev, 0_s);
}
/**

2
photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h
View File

@@ -35,7 +35,7 @@
#include <opencv2/imgcodecs.hpp>
#include <opencv2/objdetect.hpp>
#include <wpi/apriltag/AprilTag.hpp>
#include <wpi/cs/cscore_cv.hpp>
#include <wpi/cs/CvSource.hpp>
#include <wpi/units/length.hpp>
#include "photon/simulation/SimCameraProperties.h"

2
photon-lib/src/test/java/org/photonvision/OpenCVTest.java
View File

@@ -102,7 +102,7 @@ public class OpenCVTest {
var rvec = OpenCVHelp.rotationToRvec(rot);
var result = OpenCVHelp.rvecToRotation(rvec);
rvec.release();
var diff = result.minus(rot);
var diff = result.relativeTo(rot);
assertSame(new Rotation3d(), diff);
}

27
photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View File

@@ -34,6 +34,7 @@ import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
import java.io.IOException;
import java.util.Arrays;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.stream.Stream;
import org.junit.jupiter.api.AfterEach;
@@ -56,6 +57,7 @@ import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.NetworkTablesJNI;
import org.wpilib.simulation.AlertSim;
import org.wpilib.simulation.SimHooks;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.DataLogManager;
@@ -322,13 +324,16 @@ class PhotonCameraTest {
// WHEN we update the camera
camera.getAllUnreadResults();
// AND we tick SmartDashboard
SmartDashboard.updateValues();
// TODO: change to getActive once that exists
String[] alertsText =
Arrays.stream(AlertSim.getAll())
.filter(it -> it.isActive())
.map(it -> it.text)
.toArray(String[]::new);
// The alert state will be set (hard-coded here)
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.anyMatch(it -> it.equals(disconnectedCameraString)));
Arrays.stream(alertsText).anyMatch(it -> Objects.equals(it, disconnectedCameraString)));
Thread.sleep(20);
}
@@ -351,16 +356,18 @@ class PhotonCameraTest {
// WHEN we update the camera
camera.getAllUnreadResults();
// AND we tick SmartDashboard
SmartDashboard.updateValues();
// TODO: change to getActive once that exists
String[] alertsText =
Arrays.stream(AlertSim.getAll())
.filter(it -> it.isActive())
.map(it -> it.text)
.toArray(String[]::new);
// THEN the camera isn't disconnected
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.noneMatch(it -> it.equals(disconnectedCameraString)));
assertTrue(Arrays.stream(alertsText).noneMatch(it -> it.equals(disconnectedCameraString)));
// AND the alert string looks like a timesync warning
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
Arrays.stream(alertsText)
.filter(it -> it.contains("is not connected to the TimeSyncServer"))
.count()
== 1);

2
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -49,13 +49,11 @@ import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Quaternion;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.VecBuilder;

168
photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp
View File

@@ -26,14 +26,14 @@
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include <wpi/deprecated.h>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
#include <wpi/util/SmallVector.hpp>
#include <wpi/util/deprecated.hpp>
#include "gtest/gtest.h"
#include "photon/PhotonCamera.h"
@@ -48,11 +48,13 @@
#include "photon/targeting/PnpResult.h"
WPI_IGNORE_DEPRECATED
namespace units = wpi::units;
static std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
wpi::math::Rotation3d())}};
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), wpi::math::Rotation3d())}};
static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
@@ -70,23 +72,23 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -113,10 +115,10 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
wpi::math::Rotation3d())},
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), wpi::math::Rotation3d())},
};
auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft);
@@ -131,23 +133,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -180,23 +182,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -206,8 +208,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
estimator.SetReferencePose(wpi::math::Pose3d(
1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -231,23 +233,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -257,8 +259,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
estimator.SetLastPose(wpi::math::Pose3d(
1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -272,23 +274,23 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
@@ -332,8 +334,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
wpi::math::Pose3d realPose =
wpi::math::Pose3d(7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad));
wpi::math::Pose3d realPose = wpi::math::Pose3d(
7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
@@ -359,12 +361,12 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
wpi::math::Transform3d straightOnTestTransform =
wpi::math::Transform3d(0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad));
wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d(
0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m,
wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad));
wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
@@ -396,23 +398,23 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -445,23 +447,23 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -520,8 +522,9 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(wpi::math::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), wpi::math::Rotation3d()));
estimator.SetReferencePose(
wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), units::meter_t(3),
wpi::math::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
@@ -539,16 +542,16 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
@@ -589,7 +592,8 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) {
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo),
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d());
photon::PhotonPipelineResult result;
@@ -613,10 +617,10 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
wpi::math::Transform3d poseTransform(
wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
wpi::math::Rotation3d(wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
0.48678786477534686_m),
wpi::math::Rotation3d(
wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695, 0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
@@ -635,11 +639,13 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m),
wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)};
const wpi::math::Transform3d kRobotToCam{
wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m),
wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo),
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),

56
photon-lib/src/test/native/cpp/PhotonCameraTest.cpp
View File

@@ -33,6 +33,7 @@
#include <photon/simulation/PhotonCameraSim.h>
#include <wpi/hal/HAL.h>
#include <wpi/nt/NetworkTableInstance.hpp>
#include <wpi/simulation/AlertSim.hpp>
#include <wpi/smartdashboard/SmartDashboard.hpp>
TEST(TimeSyncProtocolTest, Smoketest) {
@@ -60,8 +61,6 @@ TEST(TimeSyncProtocolTest, Smoketest) {
}
TEST(PhotonCameraTest, Alerts) {
using wpi::SmartDashboard;
// GIVEN a local-only NT instance
auto inst = wpi::nt::NetworkTableInstance::GetDefault();
inst.StopClient();
@@ -78,20 +77,29 @@ TEST(PhotonCameraTest, Alerts) {
std::string disconnectedCameraString =
"PhotonCamera '" + cameraName + "' is disconnected.";
// TODO: Replace this when getActive is upstreamed to wpilib
auto getActiveAlerts = []() {
auto infos = wpi::sim::AlertSim::GetAll();
std::erase_if(infos, [](const wpi::sim::AlertSim::AlertInfo& info) {
return !info.isActive();
});
return infos;
};
// Loop to hit cases past first iteration
for (int i = 0; i < 10; i++) {
// WHEN we update the camera
camera.GetAllUnreadResults();
// AND we tick SmartDashboard
SmartDashboard::UpdateValues();
// The alert state will be set (hard-coded here)
auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {});
EXPECT_TRUE(
std::any_of(alerts.begin(), alerts.end(),
[&disconnectedCameraString](const std::string& alert) {
return alert == disconnectedCameraString;
}));
auto alerts = getActiveAlerts();
EXPECT_TRUE(std::any_of(alerts.begin(), alerts.end(),
[&disconnectedCameraString](
const wpi::sim::AlertSim::AlertInfo& alert) {
return alert.text == disconnectedCameraString;
}));
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}
@@ -109,25 +117,23 @@ TEST(PhotonCameraTest, Alerts) {
sim.SubmitProcessedFrame(noPongResult);
// WHEN we update the camera
camera.GetAllUnreadResults();
// AND we tick SmartDashboard
SmartDashboard::UpdateValues();
// THEN the camera isn't disconnected
auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {});
fmt::println("{}:{}: saw alerts: {}", __FILE__, __LINE__, alerts);
EXPECT_TRUE(
std::none_of(alerts.begin(), alerts.end(),
[&disconnectedCameraString](const std::string& alert) {
return alert == disconnectedCameraString;
}));
auto alerts = getActiveAlerts();
EXPECT_TRUE(std::none_of(alerts.begin(), alerts.end(),
[&disconnectedCameraString](
const wpi::sim::AlertSim::AlertInfo& alert) {
return alert.text == disconnectedCameraString;
}));
// AND the alert string looks like a timesync warning
EXPECT_EQ(
1, std::count_if(
alerts.begin(), alerts.end(), [](const std::string& alert) {
return alert.find("is not connected to the TimeSyncServer") !=
std::string::npos;
}));
EXPECT_EQ(1, std::count_if(
alerts.begin(), alerts.end(),
[](const wpi::sim::AlertSim::AlertInfo& alert) {
return alert.text.find(
"is not connected to the TimeSyncServer") !=
std::string::npos;
}));
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

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

@@ -205,8 +205,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
result, wpi::math::Pose3d(1_m, 1_m, 1_m,
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
@@ -493,10 +493,15 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d());
wpi::math::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
auto distortion = Eigen::VectorXd::Zero(8);
auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
{0, 399.16666666666674, 239.5},
{0, 0, 1}};
auto estimate = estimator.EstimateConstrainedSolvepnpPose(
result, cameraMat, distortion, wpi::math::Pose3d(), true, 0.0);
EXPECT_FALSE(estimate.has_value());
}

4
photon-lib/src/test/native/cpp/VersionTest.cpp
View File

@@ -23,10 +23,10 @@
*/
#include <gtest/gtest.h>
#include <wpi/print.h>
#include <wpi/util/print.hpp>
#include "PhotonVersion.h"
TEST(VersionTest, PrintVersion) {
wpi::println("{}", photon::PhotonVersion::versionString);
wpi::util::println("{}", photon::PhotonVersion::versionString);
}

139
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -50,13 +50,14 @@ class VisionSystemSimTestWithParamsTest
public testing::WithParamInterface<wpi::units::degree_t> {};
class VisionSystemSimTestDistanceParamsTest
: public VisionSystemSimTest,
public testing::WithParamInterface<
std::tuple<wpi::units::foot_t, wpi::units::degree_t, wpi::units::foot_t>> {};
public testing::WithParamInterface<std::tuple<
wpi::units::foot_t, wpi::units::degree_t, wpi::units::foot_t>> {};
TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
@@ -110,10 +111,11 @@ TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) {
// Now walk it by yourself
visionSysSim.AdjustCamera(
&cameraSim, wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
&cameraSim,
wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{0_deg, 0_deg,
wpi::units::radian_t{std::numbers::pi}}});
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
}
@@ -122,20 +124,22 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) {
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
std::vector<photon::VisionTargetSim> targets;
for (int i = 0; i < 100; i++) {
targets.emplace_back(
frc::Pose3d{
frc::Translation3d{15.98_m + i * 0.1_m, 0_m, 1_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}},
wpi::math::Pose3d{
wpi::math::Translation3d{15.98_m + i * 0.1_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}},
photon::TargetModel{0.5_m, 0.5_m}, i);
}
visionSysSim.AddVisionTargets(targets);
frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_EQ(camera.GetLatestResult().targets.size(), 50u);
@@ -144,7 +148,8 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) {
TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
@@ -160,10 +165,11 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
visionSysSim.AdjustCamera(
&cameraSim, wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 5000_m},
wpi::math::Rotation3d{
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
&cameraSim,
wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 5000_m},
wpi::math::Rotation3d{0_deg, 0_deg,
wpi::units::radian_t{std::numbers::pi}}});
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
}
@@ -171,7 +177,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
TEST_F(VisionSystemSimTest, TestNotVisibleVert2) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Transform3d robotToCamera{
wpi::math::Translation3d{0_m, 0_m, 1_m},
@@ -200,7 +207,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert2) {
TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
@@ -225,7 +233,8 @@ TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) {
TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
@@ -291,10 +300,10 @@ TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{10_m, 0_m},
wpi::math::Rotation2d{-1 * GetParam()}};
visionSysSim.AdjustCamera(
&cameraSim,
wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{0_rad, wpi::units::degree_t{GetParam()}, 0_rad}});
&cameraSim, wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{
0_rad, wpi::units::degree_t{GetParam()}, 0_rad}});
visionSysSim.Update(robotPose);
const auto result = camera.GetLatestResult();
@@ -370,13 +379,16 @@ INSTANTIATE_TEST_SUITE_P(
TEST_F(VisionSystemSimTest, TestMultipleTargets) {
wpi::math::Pose3d targetPoseL{
wpi::math::Translation3d{15.98_m, 2_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPoseC{
wpi::math::Translation3d{15.98_m, 0_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPoseR{
wpi::math::Translation3d{15.98_m, -2_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
@@ -449,20 +461,20 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
std::vector<wpi::apriltag::AprilTag> tagList;
tagList.emplace_back(wpi::apriltag::AprilTag{
0,
wpi::math::Pose3d{12_m, 3_m, 1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
0, wpi::math::Pose3d{
12_m, 3_m, 1_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
1,
wpi::math::Pose3d{12_m, 1_m, -1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
1, wpi::math::Pose3d{
12_m, 1_m, -1_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
2,
wpi::math::Pose3d{11_m, 0_m, 2_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
2, wpi::math::Pose3d{
11_m, 0_m, 2_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
wpi::units::meter_t fieldLength{54};
wpi::units::meter_t fieldWidth{27};
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
@@ -488,8 +500,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
ASSERT_NEAR(
wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}});
@@ -510,8 +523,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
ASSERT_NEAR(
wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}
TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
@@ -528,20 +542,20 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
std::vector<wpi::apriltag::AprilTag> tagList;
tagList.emplace_back(wpi::apriltag::AprilTag{
0,
wpi::math::Pose3d{12_m, 3_m, 1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
0, wpi::math::Pose3d{
12_m, 3_m, 1_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
1,
wpi::math::Pose3d{12_m, 1_m, -1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
1, wpi::math::Pose3d{
12_m, 1_m, -1_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
2,
wpi::math::Pose3d{11_m, 0_m, 2_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
2, wpi::math::Pose3d{
11_m, 0_m, 2_m,
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}}});
wpi::units::meter_t fieldLength{54};
wpi::units::meter_t fieldWidth{27};
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
@@ -572,8 +586,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
ASSERT_NEAR(
wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag36h11, 1}});
@@ -595,8 +610,9 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
ASSERT_NEAR(
wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}
TEST_F(VisionSystemSimTest, TestTagAmbiguity) {
@@ -609,7 +625,8 @@ TEST_F(VisionSystemSimTest, TestTagAmbiguity) {
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{2_m, 0_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi}}};
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}});