mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
29e24bbac2
This adds support for PNP_DISTANCE_TRIG_SOLVE in the the python PhotonPoseEstimator, mirroring the implementation in the Java PhotonPoseEstimator. Changes: - Add PoseStrategy.PNP_DISTANCE_TRIG_SOLVE - Add addHeadingData() and resetHeadingData() to PhotonPoseEstimator - Fix PhotonCameraSim.process() to set ntReceiveTimestampMicros in the result - Minor readability improvements to PhotonPipelineResult - Minor test improvements to PhotonPoseEstimatorTest - Add .vscode/settings.json (to make running python tests in VSCode easier) 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 v2024.3.1 - [ ] 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 --------- Co-authored-by: Sam948-byte <samf.236@proton.me>
71 lines
2.6 KiB
Python
71 lines
2.6 KiB
Python
from dataclasses import dataclass, field
|
|
from typing import TYPE_CHECKING, ClassVar, Optional
|
|
|
|
from .multiTargetPNPResult import MultiTargetPNPResult
|
|
from .photonTrackedTarget import PhotonTrackedTarget
|
|
|
|
if TYPE_CHECKING:
|
|
from ..generated.PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde
|
|
from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde
|
|
|
|
|
|
@dataclass
|
|
class PhotonPipelineMetadata:
|
|
# Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
|
|
# reported by WPIUtilJNI::now.
|
|
captureTimestampMicros: int = -1
|
|
publishTimestampMicros: int = -1
|
|
|
|
# Mirror of the heartbeat entry -- monotonically increasing
|
|
sequenceID: int = -1
|
|
|
|
timeSinceLastPong: int = -1
|
|
|
|
photonStruct: ClassVar["PhotonPipelineMetadataSerde"]
|
|
|
|
|
|
@dataclass
|
|
class PhotonPipelineResult:
|
|
# Since we don't trust NT time sync, keep track of when we got this packet into robot code
|
|
ntReceiveTimestampMicros: int = -1
|
|
|
|
targets: list[PhotonTrackedTarget] = field(default_factory=list)
|
|
# Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
|
|
# an arbitrary timebase. This is not true in C++ or Java.
|
|
metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
|
|
multitagResult: Optional[MultiTargetPNPResult] = None
|
|
|
|
def getLatencyMillis(self) -> float:
|
|
return (
|
|
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
|
|
) / 1e3
|
|
|
|
def getTimestampSeconds(self) -> float:
|
|
"""
|
|
Returns the estimated time the frame was taken, in the Received system's time base. This is
|
|
calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
|
|
timestamp, coproc timebase))
|
|
"""
|
|
# TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
|
|
latency = (
|
|
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
|
|
)
|
|
return (self.ntReceiveTimestampMicros - latency) / 1e6
|
|
|
|
def getTargets(self) -> list[PhotonTrackedTarget]:
|
|
return self.targets
|
|
|
|
def hasTargets(self) -> bool:
|
|
return len(self.targets) > 0
|
|
|
|
def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
|
|
"""
|
|
Returns the best target in this pipeline result. If there are no targets, this method will
|
|
return null. The best target is determined by the target sort mode in the PhotonVision UI.
|
|
"""
|
|
if not self.hasTargets():
|
|
return None
|
|
return self.getTargets()[0]
|
|
|
|
photonStruct: ClassVar["PhotonPipelineResultSerde"]
|