Clarify when estimate methods return empty (#2323)

## Description

Clarifies when estimate methods return an empty optional. This also
fixes some parity issues with Constrained SolvePnP not checking the
optional returned by the heading buffer.

Resolves #2322.

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2025.3.2
- [ ] If this PR touches pipeline settings or anything related to data
exchange, the frontend typing is updated
- [ ] If this PR addresses a bug, a regression test for it is added
This commit is contained in:
Gold856
2026-01-21 18:13:47 -05:00
committed by GitHub
parent 12f74423d9
commit e73420d62a
4 changed files with 46 additions and 22 deletions

View File

@@ -634,13 +634,18 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
// Need heading if heading fixed
if (!headingFree) {
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
if (!headingBuffer.Sample(cameraResult.GetTimestamp())) {
return std::nullopt;
} else {
// If heading fixed, force rotation component
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
}
std::vector<photon::PhotonTrackedTarget> targets{
cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};