Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-29 02:21:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
photon-lib/build.gradle
View File

@@ -18,7 +18,25 @@ apply from: "${rootDir}/versioningHelper.gradle"
nativeUtils {
exportsConfigs {
"${nativeName}" {}
"${nativeName}" {
// From https://github.com/wpilibsuite/allwpilib/blob/a32589831184969939fd3d63f449a2788a0a8542/wpimath/build.gradle#L72
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
x64ExcludeSymbols = [
'_CT??_R0?AV_System_error',
'_CT??_R0?AVexception',
'_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error',
'_CT??_R0?AVsystem_error',
'_CTA5?AVfailure',
'_TI5?AVfailure',
'_CT??_R0?AVout_of_range',
'_CTA3?AVout_of_range',
'_TI3?AVout_of_range',
'_CT??_R0?AVbad_cast'
]
}
}
}

14
photon-lib/py/buildAndTest.sh Executable file
View File

@@ -0,0 +1,14 @@
# Uninstall if it already was installed
python3 -m pip uninstall -y photonlibpy
# Build wheel
python3 setup.py bdist_wheel
# Install whatever wheel was made
for f in dist/*.whl; do
echo "installing $f"
python3 -m pip install --no-cache-dir "$f"
done
# Run the test suite
pytest -rP --full-trace

5
photon-lib/py/photonlibpy/__init__.py
View File

@@ -1 +1,6 @@
# No one here but us chickens
from .packet import Packet # noqa
from .estimatedRobotPose import EstimatedRobotPose # noqa
from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy # noqa
from .photonCamera import PhotonCamera # noqa

2
photon-lib/py/photonlibpy/estimatedRobotPose.py
View File

@@ -3,7 +3,7 @@ from typing import TYPE_CHECKING
from wpimath.geometry import Pose3d
from .photonTrackedTarget import PhotonTrackedTarget
from .targeting.photonTrackedTarget import PhotonTrackedTarget
if TYPE_CHECKING:
from .photonPoseEstimator import PoseStrategy

46
photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py Normal file
View File

@@ -0,0 +1,46 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class MultiTargetPNPResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b"
MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"
@staticmethod
def unpack(packet: "Packet") -> "MultiTargetPNPResult":
ret = MultiTargetPNPResult()
# estimatedPose is of non-intrinsic type PnpResult
ret.estimatedPose = PnpResult.photonStruct.unpack(packet)
# fiducialIDsUsed is a custom VLA!
ret.fiducialIDsUsed = packet.decodeShortList()
return ret
# Hack ourselves into the base class
MultiTargetPNPResult.photonStruct = MultiTargetPNPResultSerde()

51
photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py Normal file
View File

@@ -0,0 +1,51 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonPipelineMetadataSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"
MESSAGE_FORMAT = (
"int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"
)
@staticmethod
def unpack(packet: "Packet") -> "PhotonPipelineMetadata":
ret = PhotonPipelineMetadata()
# sequenceID is of intrinsic type int64
ret.sequenceID = packet.decodeLong()
# captureTimestampMicros is of intrinsic type int64
ret.captureTimestampMicros = packet.decodeLong()
# publishTimestampMicros is of intrinsic type int64
ret.publishTimestampMicros = packet.decodeLong()
return ret
# Hack ourselves into the base class
PhotonPipelineMetadata.photonStruct = PhotonPipelineMetadataSerde()

49
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py Normal file
View File

@@ -0,0 +1,49 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonPipelineResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2"
MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multiTagResult;"
@staticmethod
def unpack(packet: "Packet") -> "PhotonPipelineResult":
ret = PhotonPipelineResult()
# metadata is of non-intrinsic type PhotonPipelineMetadata
ret.metadata = PhotonPipelineMetadata.photonStruct.unpack(packet)
# targets is a custom VLA!
ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct)
# multiTagResult is optional! it better not be a VLA too
ret.multiTagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)
return ret
# Hack ourselves into the base class
PhotonPipelineResult.photonStruct = PhotonPipelineResultSerde()

76
photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py Normal file
View File

@@ -0,0 +1,76 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonTrackedTargetSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60"
MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] detectedCorners;"
@staticmethod
def unpack(packet: "Packet") -> "PhotonTrackedTarget":
ret = PhotonTrackedTarget()
# yaw is of intrinsic type float64
ret.yaw = packet.decodeDouble()
# pitch is of intrinsic type float64
ret.pitch = packet.decodeDouble()
# area is of intrinsic type float64
ret.area = packet.decodeDouble()
# skew is of intrinsic type float64
ret.skew = packet.decodeDouble()
# fiducialId is of intrinsic type int32
ret.fiducialId = packet.decodeInt()
# objDetectId is of intrinsic type int32
ret.objDetectId = packet.decodeInt()
# objDetectConf is of intrinsic type float32
ret.objDetectConf = packet.decodeFloat()
# field is shimmed!
ret.bestCameraToTarget = packet.decodeTransform()
# field is shimmed!
ret.altCameraToTarget = packet.decodeTransform()
# poseAmbiguity is of intrinsic type float64
ret.poseAmbiguity = packet.decodeDouble()
# minAreaRectCorners is a custom VLA!
ret.minAreaRectCorners = packet.decodeList(TargetCorner.photonStruct)
# detectedCorners is a custom VLA!
ret.detectedCorners = packet.decodeList(TargetCorner.photonStruct)
return ret
# Hack ourselves into the base class
PhotonTrackedTarget.photonStruct = PhotonTrackedTargetSerde()

55
photon-lib/py/photonlibpy/generated/PnpResultSerde.py Normal file
View File

@@ -0,0 +1,55 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PnpResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491"
MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
@staticmethod
def unpack(packet: "Packet") -> "PnpResult":
ret = PnpResult()
# field is shimmed!
ret.best = packet.decodeTransform()
# field is shimmed!
ret.alt = packet.decodeTransform()
# bestReprojErr is of intrinsic type float64
ret.bestReprojErr = packet.decodeDouble()
# altReprojErr is of intrinsic type float64
ret.altReprojErr = packet.decodeDouble()
# ambiguity is of intrinsic type float64
ret.ambiguity = packet.decodeDouble()
return ret
# Hack ourselves into the base class
PnpResult.photonStruct = PnpResultSerde()

46
photon-lib/py/photonlibpy/generated/TargetCornerSerde.py Normal file
View File

@@ -0,0 +1,46 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class TargetCornerSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8"
MESSAGE_FORMAT = "float64 x;float64 y;"
@staticmethod
def unpack(packet: "Packet") -> "TargetCorner":
ret = TargetCorner()
# x is of intrinsic type float64
ret.x = packet.decodeDouble()
# y is of intrinsic type float64
ret.y = packet.decodeDouble()
return ret
# Hack ourselves into the base class
TargetCorner.photonStruct = TargetCornerSerde()

9
photon-lib/py/photonlibpy/generated/__init__.py Normal file
View File

@@ -0,0 +1,9 @@
# no one but us chickens
from .MultiTargetPNPResultSerde import MultiTargetPNPResultSerde # noqa
from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde # noqa
from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde # noqa
from .PhotonPipelineResultSerde import PhotonPipelineResultSerde # noqa
from .PhotonTrackedTargetSerde import PhotonTrackedTargetSerde # noqa
from .PnpResultSerde import PnpResultSerde # noqa
from .TargetCornerSerde import TargetCornerSerde # noqa

49
photon-lib/py/photonlibpy/multiTargetPNPResult.py
View File

@@ -1,49 +0,0 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from photonlibpy.packet import Packet
@dataclass
class PNPResult:
_NUM_BYTES_IN_FLOAT = 8
PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3)
isPresent: bool = False
best: Transform3d = field(default_factory=Transform3d)
alt: Transform3d = field(default_factory=Transform3d)
ambiguity: float = 0.0
bestReprojError: float = 0.0
altReprojError: float = 0.0
def createFromPacket(self, packet: Packet) -> Packet:
self.isPresent = packet.decodeBoolean()
if not self.isPresent:
return packet
self.best = packet.decodeTransform()
self.alt = packet.decodeTransform()
self.bestReprojError = packet.decodeDouble()
self.altReprojError = packet.decodeDouble()
self.ambiguity = packet.decodeDouble()
return packet
@dataclass
class MultiTargetPNPResult:
_MAX_IDS = 32
# pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
_PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS)
estimatedPose: PNPResult = field(default_factory=PNPResult)
fiducialIDsUsed: list[int] = field(default_factory=list)
def createFromPacket(self, packet: Packet) -> Packet:
self.estimatedPose = PNPResult()
self.estimatedPose.createFromPacket(packet)
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet

34
photon-lib/py/photonlibpy/packet.py
View File

@@ -1,4 +1,5 @@
import struct
from typing import Any, Optional, Type
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
import wpilib
@@ -82,13 +83,13 @@ class Packet:
def decode16(self) -> int:
"""
* Returns a single decoded byte from the packet.
* Returns a single decoded short from the packet.
*
* @return A decoded byte from the packet.
* @return A decoded short from the packet.
"""
return self._decodeGeneric(">h", 2)
def decode32(self) -> int:
def decodeInt(self) -> int:
"""
* Returns a decoded int (32 bytes) from the packet.
*
@@ -104,7 +105,7 @@ class Packet:
"""
return self._decodeGeneric(">f", 4)
def decodei64(self) -> int:
def decodeLong(self) -> int:
"""
* Returns a decoded int64 from the packet.
*
@@ -131,14 +132,22 @@ class Packet:
def decodeDoubleArray(self, length: int) -> list[float]:
"""
* Returns a decoded array of floats from the packet.
*
* @return A decoded array of floats from the packet.
"""
ret = []
for _ in range(length):
ret.append(self.decodeDouble())
return ret
def decodeShortList(self) -> list[float]:
"""
* Returns a decoded array of shorts from the packet.
"""
length = self.decode8()
ret = []
for _ in range(length):
ret.append(self.decode16())
return ret
def decodeTransform(self) -> Transform3d:
"""
* Returns a decoded Transform3d
@@ -157,3 +166,16 @@ class Packet:
rotation = Rotation3d(Quaternion(w, x, y, z))
return Transform3d(translation, rotation)
def decodeList(self, serde: Type):
retList = []
arr_len = self.decode8()
for _ in range(arr_len):
retList.append(serde.unpack(self))
return retList
def decodeOptional(self, serde: Type) -> Optional[Any]:
if self.decodeBoolean():
return serde.unpack(self)
else:
return None

26
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -3,9 +3,12 @@ from typing import List
import ntcore
from wpilib import RobotController, Timer
import wpilib
from photonlibpy.packet import Packet
from photonlibpy.photonPipelineResult import PhotonPipelineResult
from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
from .packet import Packet
from .targeting.photonPipelineResult import PhotonPipelineResult
from .version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
# magical import to make serde stuff work
import photonlibpy.generated # noqa
class VisionLEDMode(Enum):
@@ -100,11 +103,9 @@ class PhotonCamera:
else:
newResult = PhotonPipelineResult()
pkt = Packet(byteList)
newResult.populateFromPacket(pkt)
newResult = PhotonPipelineResult.photonStruct.unpack(pkt)
# NT4 allows us to correct the timestamp based on when the message was sent
newResult.setTimestampSeconds(
timestamp / 1e6 - newResult.getLatencyMillis() / 1e3
)
newResult.ntReceiveTimestampMicros = timestamp / 1e6
ret.append(newResult)
return ret
@@ -113,18 +114,17 @@ class PhotonCamera:
self._versionCheck()
now = RobotController.getFPGATime()
retVal = PhotonPipelineResult()
packetWithTimestamp = self._rawBytesEntry.getAtomic()
byteList = packetWithTimestamp.value
timestamp = packetWithTimestamp.time
packetWithTimestamp.time
if len(byteList) < 1:
return retVal
return PhotonPipelineResult()
else:
pkt = Packet(byteList)
retVal.populateFromPacket(pkt)
retVal = PhotonPipelineResult.photonStruct.unpack(pkt)
# We don't trust NT4 time, hack around
retVal.ntRecieveTimestampMicros = now
retVal.ntReceiveTimestampMicros = now
return retVal
def getDriverMode(self) -> bool:
@@ -233,6 +233,6 @@ class PhotonCamera:
wpilib.reportWarning(bfw)
errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {PHOTONLIB_VERSION}."
errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {versionString}, or update your coprocessor to {PHOTONLIB_VERSION}."
wpilib.reportError(errText, True)
raise Exception(errText)

2
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -5,7 +5,7 @@ import wpilib
from robotpy_apriltag import AprilTagFieldLayout
from wpimath.geometry import Transform3d, Pose3d, Pose2d
from .photonPipelineResult import PhotonPipelineResult
from .targeting.photonPipelineResult import PhotonPipelineResult
from .photonCamera import PhotonCamera
from .estimatedRobotPose import EstimatedRobotPose

9
photon-lib/py/photonlibpy/targeting/TargetCorner.py Normal file
View File

@@ -0,0 +1,9 @@
from dataclasses import dataclass
@dataclass
class TargetCorner:
x: float = 0
y: float = 9
photonStruct: "TargetCornerSerde" = None

6
photon-lib/py/photonlibpy/targeting/__init__.py Normal file
View File

@@ -0,0 +1,6 @@
# no one but us chickens
from .TargetCorner import TargetCorner # noqa
from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult # noqa
from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult # noqa
from .photonTrackedTarget import PhotonTrackedTarget # noqa

34
photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py Normal file
View File

@@ -0,0 +1,34 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from ..packet import Packet
@dataclass
class PnpResult:
best: Transform3d = field(default_factory=Transform3d)
alt: Transform3d = field(default_factory=Transform3d)
ambiguity: float = 0.0
bestReprojError: float = 0.0
altReprojError: float = 0.0
photonStruct: "PNPResultSerde" = None
@dataclass
class MultiTargetPNPResult:
_MAX_IDS = 32
estimatedPose: PnpResult = field(default_factory=PnpResult)
fiducialIDsUsed: list[int] = field(default_factory=list)
def createFromPacket(self, packet: Packet) -> Packet:
self.estimatedPose = PnpResult()
self.estimatedPose.createFromPacket(packet)
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet
photonStruct: "MultiTargetPNPResultSerde" = None

60
photon-lib/py/photonlibpy/photonPipelineResult.py → photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
View File

@@ -1,12 +1,12 @@
from dataclasses import dataclass, field
from typing import Optional
from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult
from photonlibpy.packet import Packet
from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
from .multiTargetPNPResult import MultiTargetPNPResult
from .photonTrackedTarget import PhotonTrackedTarget
@dataclass
class PhotonPipelineResult:
class PhotonPipelineMetadata:
# Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
# reported by WPIUtilJNI::now.
captureTimestampMicros: int = -1
@@ -15,49 +15,44 @@ class PhotonPipelineResult:
# Mirror of the heartbeat entry -- monotonically increasing
sequenceID: int = -1
photonStruct: "PhotonPipelineMetadataSerde" = None
@dataclass
class PhotonPipelineResult:
# Since we don't trust NT time sync, keep track of when we got this packet into robot code
ntRecieveTimestampMicros: int = -1
ntReceiveTimestampMicros: int = -1
targets: list[PhotonTrackedTarget] = field(default_factory=list)
multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult)
def populateFromPacket(self, packet: Packet) -> Packet:
self.targets = []
self.sequenceID = packet.decodei64()
self.captureTimestampMicros = packet.decodei64()
self.publishTimestampMicros = packet.decodei64()
targetCount = packet.decode8()
for _ in range(targetCount):
target = PhotonTrackedTarget()
target.createFromPacket(packet)
self.targets.append(target)
self.multiTagResult = MultiTargetPNPResult()
self.multiTagResult.createFromPacket(packet)
return packet
metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
multiTagResult: Optional[MultiTargetPNPResult] = None
def getLatencyMillis(self) -> float:
return (self.publishTimestampMicros - self.captureTimestampMicros) / 1e3
return (
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
) / 1e3
def getTimestampSeconds(self) -> float:
"""
Returns the estimated time the frame was taken, in the recieved system's time base. This is
calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture
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 ntRecieveTimestampMicros
# TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
return (
self.ntRecieveTimestampMicros
- (self.publishTimestampMicros - self.captureTimestampMicros)
self.ntReceiveTimestampMicros
- (
self.metadata.publishTimestampMicros
- self.metadata.captureTimestampMicros
)
) / 1e6
def getTargets(self) -> list[PhotonTrackedTarget]:
return self.targets
def hasTargets(self) -> bool:
return len(self.targets) > 0
def getBestTarget(self) -> PhotonTrackedTarget:
"""
Returns the best target in this pipeline result. If there are no targets, this method will
@@ -67,5 +62,4 @@ class PhotonPipelineResult:
return None
return self.getTargets()[0]
def hasTargets(self) -> bool:
return len(self.targets) > 0
photonStruct: "PhotonPipelineResultSerde" = None

33
photon-lib/py/photonlibpy/photonTrackedTarget.py → photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py
View File

@@ -1,20 +1,11 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from photonlibpy.packet import Packet
@dataclass
class TargetCorner:
x: float
y: float
from ..packet import Packet
from .TargetCorner import TargetCorner
@dataclass
class PhotonTrackedTarget:
_MAX_CORNERS = 8
_NUM_BYTES_IN_FLOAT = 8
_PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS)
yaw: float = 0.0
pitch: float = 0.0
area: float = 0.0
@@ -64,22 +55,4 @@ class PhotonTrackedTarget:
retList.append(TargetCorner(cx, cy))
return retList
def createFromPacket(self, packet: Packet) -> Packet:
self.yaw = packet.decodeDouble()
self.pitch = packet.decodeDouble()
self.area = packet.decodeDouble()
self.skew = packet.decodeDouble()
self.fiducialId = packet.decode32()
self.classId = packet.decode32()
self.objDetectConf = packet.decodeFloat()
self.bestCameraToTarget = packet.decodeTransform()
self.altCameraToTarget = packet.decodeTransform()
self.poseAmbiguity = packet.decodeDouble()
self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four
numCorners = packet.decode8()
self.detectedCorners = self._decodeTargetList(packet, numCorners)
return packet
photonStruct: "PhotonTrackedTargetSerde" = None

5
photon-lib/py/setup.py
View File

@@ -48,10 +48,7 @@ with open("photonlibpy/version.py", "w", encoding="utf-8") as fp:
fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n')
descriptionStr = f"""
Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors.
Implemented with PhotonVision version {gitDescribeResult} .
"""
descriptionStr = f"Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version {gitDescribeResult} ."
setup(
name="photonlibpy",

439
photon-lib/py/test/photonPoseEstimator_test.py
View File

@@ -1,251 +1,244 @@
from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult, PNPResult
from photonlibpy.photonPipelineResult import PhotonPipelineResult
from photonlibpy.photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
from photonlibpy.photonTrackedTarget import PhotonTrackedTarget, TargetCorner
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
# from photonlibpy import MultiTargetPNPResult, PnpResult
# from photonlibpy import PhotonPipelineResult
# from photonlibpy import PhotonPoseEstimator, PoseStrategy
# from photonlibpy import PhotonTrackedTarget, TargetCorner, PhotonPipelineMetadata
# from robotpy_apriltag import AprilTag, AprilTagFieldLayout
# from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
class PhotonCameraInjector:
result: PhotonPipelineResult
# class PhotonCameraInjector:
# result: PhotonPipelineResult
def getLatestResult(self) -> PhotonPipelineResult:
return self.result
# def getLatestResult(self) -> PhotonPipelineResult:
# return self.result
def setupCommon() -> AprilTagFieldLayout:
tagList = []
tagPoses = (
Pose3d(3, 3, 3, Rotation3d()),
Pose3d(5, 5, 5, Rotation3d()),
)
for id_, pose in enumerate(tagPoses):
aprilTag = AprilTag()
aprilTag.ID = id_
aprilTag.pose = pose
tagList.append(aprilTag)
# def setupCommon() -> AprilTagFieldLayout:
# tagList = []
# tagPoses = (
# Pose3d(3, 3, 3, Rotation3d()),
# Pose3d(5, 5, 5, Rotation3d()),
# )
# for id_, pose in enumerate(tagPoses):
# aprilTag = AprilTag()
# aprilTag.ID = id_
# aprilTag.pose = pose
# tagList.append(aprilTag)
fieldLength = 54 / 3.281 # 54 ft -> meters
fieldWidth = 27 / 3.281 # 24 ft -> meters
# fieldLength = 54 / 3.281 # 54 ft -> meters
# fieldWidth = 27 / 3.281 # 24 ft -> meters
return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
# return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
def test_lowestAmbiguityStrategy():
aprilTags = setupCommon()
# def test_lowestAmbiguityStrategy():
# aprilTags = setupCommon()
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
0,
2 * 1e3,
1,
11 * 1e6,
[
PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.7,
),
PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.3,
),
PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.4,
),
],
)
# cameraOne = PhotonCameraInjector()
# cameraOne.result = PhotonPipelineResult(
# 11 * 1e6,
# [
# PhotonTrackedTarget(
# 3.0,
# -4.0,
# 9.0,
# 4.0,
# 0,
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# 0.7,
# ),
# PhotonTrackedTarget(
# 3.0,
# -4.0,
# 9.1,
# 6.7,
# 1,
# Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
# Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# 0.3,
# ),
# PhotonTrackedTarget(
# 9.0,
# -2.0,
# 19.0,
# 3.0,
# 0,
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# 0.4,
# ),
# ],
# None,
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
# )
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# estimator = PhotonPoseEstimator(
# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
# )
estimatedPose = estimator.update()
pose = estimatedPose.estimatedPose
# estimatedPose = estimator.update()
# pose = estimatedPose.estimatedPose
assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
assertEquals(1, pose.x, 0.01)
assertEquals(3, pose.y, 0.01)
assertEquals(2, pose.z, 0.01)
# assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
# assertEquals(1, pose.x, 0.01)
# assertEquals(3, pose.y, 0.01)
# assertEquals(2, pose.z, 0.01)
def test_multiTagOnCoprocStrategy():
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
0,
2 * 1e3,
1,
11 * 1e6,
# There needs to be at least one target present for pose estimation to work
# Doesn't matter which/how many targets for this test
[
PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.7,
)
],
multiTagResult=MultiTargetPNPResult(
PNPResult(True, Transform3d(1, 3, 2, Rotation3d()))
),
)
# def test_multiTagOnCoprocStrategy():
# cameraOne = PhotonCameraInjector()
# cameraOne.result = PhotonPipelineResult(
# 11 * 1e6,
# # There needs to be at least one target present for pose estimation to work
# # Doesn't matter which/how many targets for this test
# [
# PhotonTrackedTarget(
# 3.0,
# -4.0,
# 9.0,
# 4.0,
# 0,
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# 0.7,
# )
# ],
# multiTagResult=MultiTargetPNPResult(
# PnpResult(True, Transform3d(1, 3, 2, Rotation3d()))
# ),
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
# )
estimator = PhotonPoseEstimator(
AprilTagFieldLayout(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cameraOne,
Transform3d(),
)
# estimator = PhotonPoseEstimator(
# AprilTagFieldLayout(),
# PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
# cameraOne,
# Transform3d(),
# )
estimatedPose = estimator.update()
pose = estimatedPose.estimatedPose
# estimatedPose = estimator.update()
# pose = estimatedPose.estimatedPose
assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
assertEquals(1, pose.x, 0.01)
assertEquals(3, pose.y, 0.01)
assertEquals(2, pose.z, 0.01)
# assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
# assertEquals(1, pose.x, 0.01)
# assertEquals(3, pose.y, 0.01)
# assertEquals(2, pose.z, 0.01)
def test_cacheIsInvalidated():
aprilTags = setupCommon()
# def test_cacheIsInvalidated():
# aprilTags = setupCommon()
cameraOne = PhotonCameraInjector()
result = PhotonPipelineResult(
0,
2 * 1e3,
1,
20 * 1e6,
[
PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.7,
)
],
)
# cameraOne = PhotonCameraInjector()
# result = PhotonPipelineResult(
# 20 * 1e6,
# [
# PhotonTrackedTarget(
# 3.0,
# -4.0,
# 9.0,
# 4.0,
# 0,
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# [
# TargetCorner(1, 2),
# TargetCorner(3, 4),
# TargetCorner(5, 6),
# TargetCorner(7, 8),
# ],
# 0.7,
# )
# ],
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
# )
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# estimator = PhotonPoseEstimator(
# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
# )
# Empty result, expect empty result
cameraOne.result = PhotonPipelineResult(
captureTimestampMicros=0, publishTimestampMicros=0, ntRecieveTimestampMicros=1e6
)
estimatedPose = estimator.update()
assert estimatedPose is None
# # Empty result, expect empty result
# cameraOne.result = PhotonPipelineResult(0)
# estimatedPose = estimator.update()
# assert estimatedPose is None
# Set actual result
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is not None
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
# # Set actual result
# cameraOne.result = result
# estimatedPose = estimator.update()
# assert estimatedPose is not None
# assertEquals(20, estimatedPose.timestampSeconds, 0.01)
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
# And again -- pose cache should mean this is empty
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is None
# Expect the old timestamp to still be here
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
# # And again -- pose cache should mean this is empty
# cameraOne.result = result
# estimatedPose = estimator.update()
# assert estimatedPose is None
# # Expect the old timestamp to still be here
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
# Set new field layout -- right after, the pose cache timestamp should be -1
estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
assertEquals(-1, estimator._poseCacheTimestampSeconds)
# Update should cache the current timestamp (20) again
cameraOne.result = result
estimatedPose = estimator.update()
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
# # Set new field layout -- right after, the pose cache timestamp should be -1
# estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
# assertEquals(-1, estimator._poseCacheTimestampSeconds)
# # Update should cache the current timestamp (20) again
# cameraOne.result = result
# estimatedPose = estimator.update()
# assertEquals(20, estimatedPose.timestampSeconds, 0.01)
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
def assertEquals(expected, actual, epsilon=0.0):
assert abs(expected - actual) <= epsilon
# def assertEquals(expected, actual, epsilon=0.0):
# assert abs(expected - actual) <= epsilon

26
photon-lib/py/test/photonlibpy_test.py
View File

@@ -1,3 +1,25 @@
from time import sleep
from photonlibpy import PhotonCamera
import ntcore
from photonlibpy.photonCamera import setVersionCheckEnabled
def test_roundTrip():
# TODO implement packet encoding, or just kill me
assert True
ntcore.NetworkTableInstance.getDefault().stopServer()
ntcore.NetworkTableInstance.getDefault().setServer("localhost")
ntcore.NetworkTableInstance.getDefault().startClient4("meme")
camera = PhotonCamera("WPI2024")
setVersionCheckEnabled(False)
for i in range(5):
sleep(0.1)
result = camera.getLatestResult()
print(result)
print(camera._rawBytesEntry.getTopic().getProperties())
if __name__ == "__main__":
test_roundTrip()

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

@@ -142,7 +142,7 @@ public class PhotonCamera implements AutoCloseable {
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -193,7 +193,7 @@ public class PhotonCamera implements AutoCloseable {
// make time sync more reliable.
for (var c : changes) {
var result = c.value;
result.setRecieveTimestampMicros(c.timestamp);
result.setReceiveTimestampMicros(c.timestamp);
ret.add(result);
}
@@ -201,7 +201,7 @@ public class PhotonCamera implements AutoCloseable {
}
/**
* Returns the latest pipeline result. This is simply the most recent result recieved via NT.
* Returns the latest pipeline result. This is simply the most recent result Received via NT.
* Calling this multiple times will always return the most recent result.
*
* <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
@@ -221,7 +221,7 @@ public class PhotonCamera implements AutoCloseable {
// contains a thing with time knowledge, set it here.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
// TODO: NT4 time sync is Not To Be Trusted, we should do something else?
result.setRecieveTimestampMicros(ret.timestamp);
result.setReceiveTimestampMicros(ret.timestamp);
return result;
}
@@ -411,9 +411,20 @@ public class PhotonCamera implements AutoCloseable {
"PhotonVision coprocessor at path " + path + " is not sending new data.", true);
}
// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Check mdef UUID
String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID();
String remote_uuid = resultSubscriber.getInterfaceUUID();
if (remote_uuid == null || remote_uuid.isEmpty()) {
// not connected yet?
DriverStation.reportWarning(
"PhotonVision coprocessor at path "
+ path
+ " has note reported a message interface UUID - is your coprocessor's camera started?",
true);
} else if (!local_uuid.equals(remote_uuid)) {
// Error on a verified version mismatch
// But stay silent otherwise
@@ -439,8 +450,14 @@ public class PhotonCamera implements AutoCloseable {
var versionMismatchMessage =
"Photon version "
+ PhotonVersion.versionString
+ " (message definition version "
+ local_uuid
+ ")"
+ " does not match coprocessor version "
+ versionString
+ " (message definition version "
+ remote_uuid
+ ")"
+ "!";
DriverStation.reportError(versionMismatchMessage, false);
throw new UnsupportedOperationException(versionMismatchMessage);

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

@@ -394,8 +394,8 @@ public class PhotonPoseEstimator {
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
if (result.getMultiTagResult().isPresent()) {
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
@@ -427,11 +427,11 @@ public class PhotonPoseEstimator {
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent)
if (!pnpResult.isPresent())
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResult.best) // field-to-camera
.plus(pnpResult.get().best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(

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

@@ -55,9 +55,9 @@ import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
/**
* A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
@@ -420,14 +420,15 @@ public class PhotonCameraSim implements AutoCloseable {
// projected target can't be detected, skip to next
if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
var pnpSim = new PNPResult();
var pnpSim = new PnpResult();
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners)
.get();
}
detectableTgts.add(
@@ -519,13 +520,13 @@ public class PhotonCameraSim implements AutoCloseable {
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
// calculate multitag results
var multitagResult = new MultiTargetPNPResult();
Optional<MultiTargetPNPResult> multitagResult = Optional.empty();
// TODO: Implement ATFL subscribing in backend
// var tagLayout = cam.getAprilTagFieldLayout();
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
List<Integer> usedIDs =
visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
List<Short> usedIDs =
visibleLayoutTags.stream().map(t -> (short) t.ID).sorted().collect(Collectors.toList());
var pnpResult =
VisionEstimation.estimateCamPosePNP(
prop.getIntrinsics(),
@@ -533,7 +534,10 @@ public class PhotonCameraSim implements AutoCloseable {
detectableTgts,
tagLayout,
TargetModel.kAprilTag36h11);
multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
if (pnpResult.isPresent()) {
multitagResult = Optional.of(new MultiTargetPNPResult(pnpResult.get(), usedIDs));
}
}
// sort target order
@@ -550,7 +554,7 @@ public class PhotonCameraSim implements AutoCloseable {
now,
detectableTgts,
multitagResult);
ret.setRecieveTimestampMicros(now);
ret.setReceiveTimestampMicros(now);
return ret;
}
@@ -573,9 +577,10 @@ public class PhotonCameraSim implements AutoCloseable {
* @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds
*/
public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) {
ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp);
ts.latencyMillisEntry.set(result.metadata.getLatencyMillis(), receiveTimestamp);
ts.resultPublisher.set(result, result.getPacketSize());
// Results are now dynamically sized, so let's guess 1024 bytes is big enough
ts.resultPublisher.set(result, 1024);
boolean hasTargets = result.hasTargets();
ts.hasTargetEntry.set(hasTargets, receiveTimestamp);

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

@@ -35,6 +35,7 @@
#include <frc/Timer.h>
#include <opencv2/core.hpp>
#include <opencv2/core/mat.hpp>
#include <wpi/json.h>
#include "PhotonVersion.h"
#include "photon/dataflow/structures/Packet.h"
@@ -121,20 +122,18 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
// Prints warning if not connected
VerifyVersion();
// Create the new result;
PhotonPipelineResult result;
// Fill the packet with latest data and populate result.
units::microsecond_t now =
units::microsecond_t(frc::RobotController::GetFPGATime());
const auto value = rawBytesEntry.Get();
if (!value.size()) return result;
if (!value.size()) return PhotonPipelineResult{};
photon::Packet packet{value};
packet >> result;
// Create the new result;
PhotonPipelineResult result = packet.Unpack<PhotonPipelineResult>();
result.SetRecieveTimestamp(now);
result.SetReceiveTimestamp(now);
return result;
}
@@ -149,8 +148,9 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
const auto changes = rawBytesEntry.ReadQueue();
// Create the new result list -- these will be updated in-place
std::vector<PhotonPipelineResult> ret(changes.size());
// Create the new result list
std::vector<PhotonPipelineResult> ret;
ret.reserve(changes.size());
for (size_t i = 0; i < changes.size(); i++) {
const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
@@ -161,13 +161,14 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
// Fill the packet with latest data and populate result.
photon::Packet packet{value.value};
auto result = packet.Unpack<PhotonPipelineResult>();
PhotonPipelineResult& result = ret[i];
packet >> result;
// TODO: NT4 timestamps are still not to be trusted. But it's the best we
// can do until we can make time sync more reliable.
result.SetRecieveTimestamp(units::microsecond_t(value.time) -
result.SetReceiveTimestamp(units::microsecond_t(value.time) -
result.GetLatency());
ret.push_back(result);
}
return ret;
@@ -209,9 +210,11 @@ std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
PhotonCamera::CameraMatrix retVal =
Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
camCoeffs.data());
return retVal;
}
return std::nullopt;
}
@@ -230,22 +233,6 @@ std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
return std::nullopt;
}
static bool VersionMatches(std::string them_str) {
std::smatch match;
std::regex versionPattern{"v[0-9]+.[0-9]+.[0-9]+"};
std::string us_str = PhotonVersion::versionString;
// Check that both versions are in the right format
if (std::regex_search(us_str, match, versionPattern) &&
std::regex_search(them_str, match, versionPattern)) {
// If they are, check string equality
return (us_str == them_str);
} else {
return false;
}
}
void PhotonCamera::VerifyVersion() {
if (!PhotonCamera::VERSION_CHECK_ENABLED) {
return;
@@ -282,13 +269,20 @@ void PhotonCamera::VerifyVersion() {
"Found the following PhotonVision cameras on NetworkTables:{}",
cameraNameOutString);
}
} else if (!VersionMatches(versionString)) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} does not match coprocessor version {}!",
PhotonVersion::versionString, versionString);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
} else {
std::string local_uuid{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
std::string remote_uuid =
rawBytesEntry.GetTopic().GetProperty("message_uuid");
if (local_uuid != remote_uuid) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} (message definition version {}) does not match "
"coprocessor version {} (message definition version {})!",
PhotonVersion::versionString, local_uuid, versionString, remote_uuid);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
}
}
}

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

@@ -100,6 +100,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return std::nullopt;
}
@@ -164,7 +166,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
if (ret) {
lastPose = ret.value().estimatedPose;
lastPose = ret->estimatedPose;
}
return ret;
}
@@ -197,8 +199,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
}
return EstimatedRobotPose{
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
@@ -220,7 +221,7 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
target.GetFiducialId());
continue;
}
frc::Pose3d const targetPose = fiducialPose.value();
frc::Pose3d const targetPose = *fiducialPose;
units::meter_t const alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
@@ -349,8 +350,8 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
if (result.MultiTagResult()) {
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
@@ -398,8 +399,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
tagCorners.has_value()) {
auto const targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
imagePoints.emplace_back(targetCorners[cornerIdx].x,
targetCorners[cornerIdx].y);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
}
}

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

@@ -201,33 +201,34 @@ PhotonPipelineResult PhotonCameraSim::Process(
continue;
}
PNPResult pnpSim{};
std::optional<photon::PnpResult> pnpSim = std::nullopt;
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
pnpSim = OpenCVHelp::SolvePNP_SQPNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(),
tgt.GetModel().GetVertices(), noisyTargetCorners);
}
std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
std::vector<TargetCorner> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
smallVec.emplace_back(static_cast<double>(corner.first),
static_cast<double>(corner.second));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt,
pnpSim.ambiguity, smallVec, cornersDouble);
tgt.objDetClassId, tgt.objDetConf,
pnpSim ? pnpSim->best : frc::Transform3d{},
pnpSim ? pnpSim->alt : frc::Transform3d{},
pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble);
}
if (videoSimRawEnabled) {
@@ -275,36 +276,27 @@ PhotonPipelineResult PhotonCameraSim::Process(
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
OpenCVHelp::CornersToPoints(detectedCornersDouble),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
OpenCVHelp::CornersToPoints(detectedCornersDouble)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
cv::LINE_AA);
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
tgt.GetMinAreaRectCorners();
auto smallVec = tgt.GetMinAreaRectCorners();
std::vector<std::pair<float, float>> cornersCopy{};
cornersCopy.reserve(4);
for (const auto& corner : smallVec) {
cornersCopy.emplace_back(
std::make_pair(static_cast<float>(corner.first),
static_cast<float>(corner.second)));
}
VideoSimUtil::DrawPoly(
OpenCVHelp::CornersToPoints(cornersCopy),
OpenCVHelp::CornersToPoints(smallVec),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
@@ -316,75 +308,81 @@ PhotonPipelineResult PhotonCameraSim::Process(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::vector<int16_t> usedIds{};
usedIds.resize(visibleLayoutTags.size());
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
auto pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
if (pnpResult) {
multiTagResults = MultiTargetPNPResult{*pnpResult, usedIds};
}
}
heartbeatCounter++;
return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
multiTagResults};
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<int64_t>()},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
uint64_t ReceiveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
ReceiveTimestamp);
Packet newPacket{};
newPacket << result;
newPacket.Pack(result);
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
ts.rawBytesEntry.Set(newPacket.GetData(), ReceiveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
ts.hasTargetEntry.Set(hasTargets, ReceiveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
ts.targetPitchEntry.Set(0.0, ReceiveTimestamp);
ts.targetYawEntry.Set(0.0, ReceiveTimestamp);
ts.targetAreaEntry.Set(0.0, ReceiveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
ts.targetSkewEntry.Set(0.0, ReceiveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
ts.targetPitchEntry.Set(bestTarget.GetPitch(), ReceiveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), ReceiveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), ReceiveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), ReceiveTimestamp);
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
std::array<double, 4> poseData{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intrinsics =
prop.GetIntrinsics();
std::span<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, ReceiveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp);
ts.subTable->GetInstance().Flush();
}

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

@@ -39,7 +39,7 @@
#include <networktables/StringTopic.h>
#include <units/time.h>
#include "photon/targeting//PhotonPipelineResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
namespace cv {
class Mat;

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

@@ -34,9 +34,9 @@
#include "photon/PhotonCamera.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"
#include "photon/targeting/PnpResult.h"
namespace photon {
enum PoseStrategy {

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

@@ -28,9 +28,9 @@
#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"
#include "photon/targeting/PnpResult.h"
namespace photon {

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

@@ -34,9 +34,9 @@
#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"
#include "photon/targeting/PnpResult.h"
namespace photon {
class PhotonUtils {

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

@@ -91,7 +91,7 @@ class PhotonCameraSim {
void SubmitProcessedFrame(const PhotonPipelineResult& result);
void SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp);
uint64_t ReceiveTimestamp);
SimCameraProperties prop;

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

@@ -177,7 +177,11 @@ public class OpenCVTest {
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
prop.getIntrinsics(),
prop.getDistCoeffs(),
target.getModel().vertices,
targetCorners)
.get();
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
@@ -212,7 +216,11 @@ public class OpenCVTest {
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
prop.getIntrinsics(),
prop.getDistCoeffs(),
target.getModel().vertices,
targetCorners)
.get();
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());

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

@@ -37,10 +37,7 @@ class PhotonCameraTest {
var packet = new Packet(1);
var ret = new PhotonPipelineResult();
packet.setData(new byte[0]);
if (packet.getSize() < 1) {
return;
}
PhotonPipelineResult.serde.pack(packet, ret);
PhotonPipelineResult.photonStruct.pack(packet, ret);
});
}
}

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

@@ -130,7 +130,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
@@ -217,7 +217,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (4 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -306,7 +306,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (17 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -396,7 +396,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -478,7 +478,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
@@ -519,7 +519,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
result.setRecieveTimestampMicros((long) (20 * 1e6));
result.setReceiveTimestampMicros((long) (20 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -529,7 +529,7 @@ class PhotonPoseEstimatorTest {
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
@@ -629,7 +629,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
cameraOne.result.setRecieveTimestampMicros(20 * 1000000);
cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(

22
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -481,11 +481,12 @@ class VisionSystemSimTest {
visionSysSim.update(robotPose);
var results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5);
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5)
.get();
Pose3d pose = new Pose3d().plus(results.best);
assertEquals(5, pose.getX(), .01);
assertEquals(1, pose.getY(), .01);
@@ -500,11 +501,12 @@ class VisionSystemSimTest {
visionSysSim.update(robotPose);
results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5);
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5)
.get();
pose = new Pose3d().plus(results.best);
assertEquals(5, pose.getX(), .01);
assertEquals(1, pose.getY(), .01);

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

@@ -39,9 +39,9 @@
#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"
#include "photon/targeting/PnpResult.h"
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -51,31 +51,33 @@ static std::vector<frc::AprilTag> tags = {
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static wpi::SmallVector<std::pair<double, double>, 4> corners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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),
@@ -83,8 +85,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
@@ -93,6 +96,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -118,23 +122,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
// ID 0 at 3,3,3
// ID 1 at 5,5,5
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
@@ -142,8 +146,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(17_s);
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
@@ -152,6 +157,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -165,23 +171,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -189,8 +195,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
@@ -202,6 +209,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -214,23 +222,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -238,8 +246,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
@@ -254,31 +263,32 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targetsThree{
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1,
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -298,23 +308,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -322,8 +332,9 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
@@ -333,6 +344,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -345,23 +357,23 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
TEST(PhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -374,8 +386,10 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
{});
// empty input, expect empty out
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -385,14 +399,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {{0, 0_s, 3_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_TRUE(estimatedPose);
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
@@ -403,3 +418,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
auto test2 = testResult;
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}

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

@@ -439,9 +439,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan) {
targets.push_back(tar);
}
photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP(
auto results = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
frc::Pose3d pose = frc::Pose3d{} + results.best;
ASSERT_TRUE(results);
frc::Pose3d pose = frc::Pose3d{} + results->best;
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);
@@ -460,11 +461,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan2) {
targets2.push_back(tar);
}
photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP(
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
frc::Pose3d pose2 = frc::Pose3d{} + results2.best;
ASSERT_NEAR(5, pose2.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
ASSERT_TRUE(results2);
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
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(units::degree_t{5}.convert<units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);