diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 3ded363bf..8c0d26679 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -30,7 +30,7 @@ jobs: os: ["ubuntu-22.04", "macos-12", "windows-2022"] python_version: # - '3.8' - - '3.9' + # - '3.9' - '3.10' # - '3.11' # - '3.12' @@ -112,9 +112,9 @@ jobs: # name: raspbian-py310 # - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04-py311 # name: raspbian-py311 - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04-py312 - name: raspbian-py312 - arch-override: linuxarm64 + # - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04-py312 + # name: raspbian-py312 + # arch-override: linuxarm64 # - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04-py38 # name: raspbian-aarch64-py38 diff --git a/photon-lib/py/buildAndTest.bat b/photon-lib/py/buildAndTest.bat old mode 100644 new mode 100755 diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py index 9308b6396..4dc185e42 100644 --- a/photon-lib/py/photonlibpy/__init__.py +++ b/photon-lib/py/photonlibpy/__init__.py @@ -15,10 +15,10 @@ ## along with this program. If not, see . ############################################################################### -from .packet import Packet # noqa -from .estimatedRobotPose import EstimatedRobotPose # noqa -from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy # noqa -from .photonCamera import PhotonCamera # noqa +# from .packet import Packet # noqa +# from .estimatedRobotPose import EstimatedRobotPose # noqa +# from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy # noqa +# from .photonCamera import PhotonCamera # noqa # force-load native libraries import ntcore diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py deleted file mode 100644 index 491f71c83..000000000 --- a/photon-lib/py/photonlibpy/estimatedRobotPose.py +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -from dataclasses import dataclass -from typing import TYPE_CHECKING - -from wpimath.geometry import Pose3d - -from .targeting.photonTrackedTarget import PhotonTrackedTarget - -if TYPE_CHECKING: - from .photonPoseEstimator import PoseStrategy - - -@dataclass -class EstimatedRobotPose: - """An estimated pose based on pipeline result""" - - estimatedPose: Pose3d - """The estimated pose""" - - timestampSeconds: float - """The estimated time the frame used to derive the robot pose was taken""" - - targetsUsed: list[PhotonTrackedTarget] - """A list of the targets used to compute this pose""" - - strategy: "PoseStrategy" - """The strategy actually used to produce this pose""" diff --git a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py deleted file mode 100644 index a40d07fe4..000000000 --- a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py deleted file mode 100644 index b5ce2d8a9..000000000 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py +++ /dev/null @@ -1,50 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py deleted file mode 100644 index f638b3bb1..000000000 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ /dev/null @@ -1,48 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py deleted file mode 100644 index c728295c6..000000000 --- a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py +++ /dev/null @@ -1,75 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py deleted file mode 100644 index aaeb74c86..000000000 --- a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py +++ /dev/null @@ -1,54 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py deleted file mode 100644 index beccc9e2d..000000000 --- a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -############################################################################### -## 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() diff --git a/photon-lib/py/photonlibpy/generated/__init__.py b/photon-lib/py/photonlibpy/generated/__init__.py deleted file mode 100644 index 7a8e89787..000000000 --- a/photon-lib/py/photonlibpy/generated/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -# 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 diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py deleted file mode 100644 index 53c3f84c5..000000000 --- a/photon-lib/py/photonlibpy/packet.py +++ /dev/null @@ -1,198 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -import struct -from typing import Any, Optional, Type -from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion -import wpilib - - -class Packet: - def __init__(self, data: bytes): - """ - * Constructs an empty packet. - * - * @param self.size The self.size of the packet buffer. - """ - self.packetData = data - self.size = len(data) - self.readPos = 0 - self.outOfBytes = False - - def clear(self): - """Clears the packet and resets the read and write positions.""" - self.packetData = [0] * self.size - self.readPos = 0 - self.outOfBytes = False - - def getSize(self): - return self.size - - _NO_MORE_BYTES_MESSAGE = """ - Photonlib - Ran out of bytes while decoding. - Make sure the version of photonvision on the coprocessor - matches the version of photonlib running in the robot code. - """ - - def _getNextByteAsInt(self) -> int: - retVal = 0x00 - - if not self.outOfBytes: - try: - retVal = 0x00FF & self.packetData[self.readPos] - self.readPos += 1 - except IndexError: - wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True) - self.outOfBytes = True - - return retVal - - def getData(self) -> bytes: - """ - * Returns the packet data. - * - * @return The packet data. - """ - return self.packetData - - def setData(self, data: bytes): - """ - * Sets the packet data. - * - * @param data The packet data. - """ - self.clear() - self.packetData = data - self.size = len(self.packetData) - - def _decodeGeneric(self, unpackFormat, numBytes): - # Read ints in from the data buffer - intList = [] - for _ in range(numBytes): - intList.append(self._getNextByteAsInt()) - - # Interpret the bytes as a floating point number - value = struct.unpack(unpackFormat, bytes(intList))[0] - - return value - - def decode8(self) -> int: - """ - * Returns a single decoded byte from the packet. - * - * @return A decoded byte from the packet. - """ - return self._decodeGeneric(">b", 1) - - def decode16(self) -> int: - """ - * Returns a single decoded short from the packet. - * - * @return A decoded short from the packet. - """ - return self._decodeGeneric(">h", 2) - - def decodeInt(self) -> int: - """ - * Returns a decoded int (32 bytes) from the packet. - * - * @return A decoded int from the packet. - """ - return self._decodeGeneric(">l", 4) - - def decodeFloat(self) -> float: - """ - * Returns a decoded float from the packet. - * - * @return A decoded float from the packet. - """ - return self._decodeGeneric(">f", 4) - - def decodeLong(self) -> int: - """ - * Returns a decoded int64 from the packet. - * - * @return A decoded int64 from the packet. - """ - return self._decodeGeneric(">q", 8) - - def decodeDouble(self) -> float: - """ - * Returns a decoded double from the packet. - * - * @return A decoded double from the packet. - """ - return self._decodeGeneric(">d", 8) - - def decodeBoolean(self) -> bool: - """ - * Returns a decoded boolean from the packet. - * - * @return A decoded boolean from the packet. - """ - return self.decode8() == 1 - - def decodeDoubleArray(self, length: int) -> list[float]: - """ - * Returns 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 - * - * @return A decoded Tansform3d from the packet. - """ - x = self.decodeDouble() - y = self.decodeDouble() - z = self.decodeDouble() - translation = Translation3d(x, y, z) - - w = self.decodeDouble() - x = self.decodeDouble() - y = self.decodeDouble() - z = self.decodeDouble() - 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 diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py deleted file mode 100644 index 32f337b20..000000000 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ /dev/null @@ -1,255 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -from enum import Enum -from typing import List -import ntcore -from wpilib import RobotController, Timer -import wpilib -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): - kDefault = -1 - kOff = 0 - kOn = 1 - kBlink = 2 - - -_lastVersionTimeCheck = 0.0 -_VERSION_CHECK_ENABLED = True - - -def setVersionCheckEnabled(enabled: bool): - global _VERSION_CHECK_ENABLED - _VERSION_CHECK_ENABLED = enabled - - -class PhotonCamera: - def __init__(self, cameraName: str): - instance = ntcore.NetworkTableInstance.getDefault() - self._name = cameraName - self._tableName = "photonvision" - photonvision_root_table = instance.getTable(self._tableName) - self._cameraTable = photonvision_root_table.getSubTable(cameraName) - self._path = self._cameraTable.getPath() - self._rawBytesEntry = self._cameraTable.getRawTopic("rawBytes").subscribe( - "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True) - ) - - self._driverModePublisher = self._cameraTable.getBooleanTopic( - "driverModeRequest" - ).publish() - self._driverModeSubscriber = self._cameraTable.getBooleanTopic( - "driverMode" - ).subscribe(False) - self._inputSaveImgEntry = self._cameraTable.getIntegerTopic( - "inputSaveImgCmd" - ).getEntry(0) - self._outputSaveImgEntry = self._cameraTable.getIntegerTopic( - "outputSaveImgCmd" - ).getEntry(0) - self._pipelineIndexRequest = self._cameraTable.getIntegerTopic( - "pipelineIndexRequest" - ).publish() - self._pipelineIndexState = self._cameraTable.getIntegerTopic( - "pipelineIndexState" - ).subscribe(0) - self._heartbeatEntry = self._cameraTable.getIntegerTopic("heartbeat").subscribe( - -1 - ) - - self._ledModeRequest = photonvision_root_table.getIntegerTopic( - "ledModeRequest" - ).publish() - self._ledModeState = photonvision_root_table.getIntegerTopic( - "ledModeState" - ).subscribe(-1) - self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe( - "" - ) - - # Existing is enough to make this multisubscriber do its thing - self.topicNameSubscriber = ntcore.MultiSubscriber( - instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True) - ) - - self._prevHeartbeat = 0 - self._prevHeartbeatChangeTime = Timer.getFPGATimestamp() - - def getAllUnreadResults(self) -> List[PhotonPipelineResult]: - """ - The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults(). - Calling this function clears the internal FIFO queue, and multiple calls to - getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to - call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so - make sure to call this frequently enough to avoid old results being discarded, too! - """ - - self._versionCheck() - - changes = self._rawBytesEntry.readQueue() - - ret = [] - - for change in changes: - byteList = change.value - timestamp = change.time - - if len(byteList) < 1: - pass - else: - newResult = PhotonPipelineResult() - pkt = Packet(byteList) - newResult = PhotonPipelineResult.photonStruct.unpack(pkt) - # NT4 allows us to correct the timestamp based on when the message was sent - newResult.ntReceiveTimestampMicros = timestamp / 1e6 - ret.append(newResult) - - return ret - - def getLatestResult(self) -> PhotonPipelineResult: - self._versionCheck() - - now = RobotController.getFPGATime() - packetWithTimestamp = self._rawBytesEntry.getAtomic() - byteList = packetWithTimestamp.value - packetWithTimestamp.time - - if len(byteList) < 1: - return PhotonPipelineResult() - else: - pkt = Packet(byteList) - retVal = PhotonPipelineResult.photonStruct.unpack(pkt) - # We don't trust NT4 time, hack around - retVal.ntReceiveTimestampMicros = now - return retVal - - def getDriverMode(self) -> bool: - return self._driverModeSubscriber.get() - - def setDriverMode(self, driverMode: bool) -> None: - self._driverModePublisher.set(driverMode) - - def takeInputSnapshot(self) -> None: - self._inputSaveImgEntry.set(self._inputSaveImgEntry.get() + 1) - - def takeOutputSnapshot(self) -> None: - self._outputSaveImgEntry.set(self._outputSaveImgEntry.get() + 1) - - def getPipelineIndex(self) -> int: - return self._pipelineIndexState.get(0) - - def setPipelineIndex(self, index: int) -> None: - self._pipelineIndexRequest.set(index) - - def getLEDMode(self) -> VisionLEDMode: - mode = self._ledModeState.get() - return VisionLEDMode(mode) - - def setLEDMode(self, led: VisionLEDMode) -> None: - self._ledModeRequest.set(led.value) - - def getName(self) -> str: - return self._name - - def isConnected(self) -> bool: - curHeartbeat = self._heartbeatEntry.get() - now = Timer.getFPGATimestamp() - - if curHeartbeat != self._prevHeartbeat: - self._prevHeartbeat = curHeartbeat - self._prevHeartbeatChangeTime = now - - return (now - self._prevHeartbeatChangeTime) < 0.5 - - def _versionCheck(self) -> None: - global _lastVersionTimeCheck - - if not _VERSION_CHECK_ENABLED: - return - - if (Timer.getFPGATimestamp() - _lastVersionTimeCheck) < 5.0: - return - - _lastVersionTimeCheck = Timer.getFPGATimestamp() - - if not self._heartbeatEntry.exists(): - cameraNames = ( - self._cameraTable.getInstance().getTable(self._tableName).getSubTables() - ) - # Look for only cameras with rawBytes entry that exists - cameraNames = list( - filter( - lambda it: self._cameraTable.getSubTable(it) - .getEntry("rawBytes") - .exists(), - cameraNames, - ) - ) - - if len(cameraNames) == 0: - wpilib.reportError( - "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", - False, - ) - else: - wpilib.reportError( - f"PhotonVision coprocessor at path {self._path} not found in Network Tables. Double check that your camera names match! Only the following camera names were found: { ''.join(cameraNames)}", - True, - ) - - elif not self.isConnected(): - wpilib.reportWarning( - f"PhotonVision coprocessor at path {self._path} is not sending new data.", - True, - ) - - versionString = self.versionEntry.get(defaultValue="") - if len(versionString) > 0 and versionString != PHOTONVISION_VERSION: - # Verified version mismatch - - bfw = """ - \n\n\n - >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> - >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - >>> - >>> You are running an incompatible version - >>> of PhotonVision on your coprocessor! - >>> - >>> This is neither tested nor supported. - >>> You MUST update PhotonVision, - >>> PhotonLib, or both. - >>> - >>> Your code will now crash. - >>> We hope your day gets better. - >>> - >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> - \n\n - """ - - wpilib.reportWarning(bfw) - - 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) diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py deleted file mode 100644 index 419c64102..000000000 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ /dev/null @@ -1,340 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -import enum -from typing import Optional - -import wpilib -from robotpy_apriltag import AprilTagFieldLayout -from wpimath.geometry import Transform3d, Pose3d, Pose2d - -from .targeting.photonPipelineResult import PhotonPipelineResult -from .photonCamera import PhotonCamera -from .estimatedRobotPose import EstimatedRobotPose - - -class PoseStrategy(enum.Enum): - """ - Position estimation strategies that can be used by the PhotonPoseEstimator class. - """ - - LOWEST_AMBIGUITY = enum.auto() - """Choose the Pose with the lowest ambiguity.""" - - CLOSEST_TO_CAMERA_HEIGHT = enum.auto() - """Choose the Pose which is closest to the camera height.""" - - CLOSEST_TO_REFERENCE_POSE = enum.auto() - """Choose the Pose which is closest to a set Reference position.""" - - CLOSEST_TO_LAST_POSE = enum.auto() - """Choose the Pose which is closest to the last pose calculated.""" - - AVERAGE_BEST_TARGETS = enum.auto() - """Return the average of the best target poses using ambiguity as weight.""" - - MULTI_TAG_PNP_ON_COPROCESSOR = enum.auto() - """ - Use all visible tags to compute a single pose estimate on coprocessor. - This option needs to be enabled on the PhotonVision web UI as well. - """ - - MULTI_TAG_PNP_ON_RIO = enum.auto() - """ - Use all visible tags to compute a single pose estimate. - This runs on the RoboRIO, and can take a lot of time. - """ - - -class PhotonPoseEstimator: - """ - The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - given timestamp on the field to produce a single robot in field pose, using the strategy set - below. Example usage can be found in our apriltagExample example project. - """ - - def __init__( - self, - fieldTags: AprilTagFieldLayout, - strategy: PoseStrategy, - camera: PhotonCamera, - robotToCamera: Transform3d, - ): - """Create a new PhotonPoseEstimator. - - :param fieldTags: A WPILib AprilTagFieldLayout linking AprilTag IDs to Pose3d objects - with respect to the FIRST field using the Field Coordinate System. - Note that setting the origin of this layout object will affect the - results from this class. - :param strategy: The strategy it should use to determine the best pose. - :param camera: PhotonCamera - :param robotToCamera: Transform3d from the center of the robot to the camera mount position (i.e., - robot ➔ camera) in the Robot Coordinate System. - """ - self._fieldTags = fieldTags - self._primaryStrategy = strategy - self._camera = camera - self.robotToCamera = robotToCamera - - self._multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY - self._reportedErrors: set[int] = set() - self._poseCacheTimestampSeconds = -1.0 - self._lastPose: Optional[Pose3d] = None - self._referencePose: Optional[Pose3d] = None - - # TODO: Implement HAL reporting - - @property - def fieldTags(self) -> AprilTagFieldLayout: - """Get the AprilTagFieldLayout being used by the PositionEstimator. - - Note: Setting the origin of this layout will affect the results from this class. - - :returns: the AprilTagFieldLayout - """ - return self._fieldTags - - @fieldTags.setter - def fieldTags(self, fieldTags: AprilTagFieldLayout): - """Set the AprilTagFieldLayout being used by the PositionEstimator. - - Note: Setting the origin of this layout will affect the results from this class. - - :param fieldTags: the AprilTagFieldLayout - """ - self._checkUpdate(self._fieldTags, fieldTags) - self._fieldTags = fieldTags - - @property - def primaryStrategy(self) -> PoseStrategy: - """Get the Position Estimation Strategy being used by the Position Estimator. - - :returns: the strategy - """ - return self._primaryStrategy - - @primaryStrategy.setter - def primaryStrategy(self, strategy: PoseStrategy): - """Set the Position Estimation Strategy used by the Position Estimator. - - :param strategy: the strategy to set - """ - self._checkUpdate(self._primaryStrategy, strategy) - self._primaryStrategy = strategy - - @property - def multiTagFallbackStrategy(self) -> PoseStrategy: - return self._multiTagFallbackStrategy - - @multiTagFallbackStrategy.setter - def multiTagFallbackStrategy(self, strategy: PoseStrategy): - """Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - NOT be MULTI_TAG_PNP - - :param strategy: the strategy to set - """ - self._checkUpdate(self._multiTagFallbackStrategy, strategy) - if ( - strategy is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - or strategy is PoseStrategy.MULTI_TAG_PNP_ON_RIO - ): - wpilib.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", - False, - ) - strategy = PoseStrategy.LOWEST_AMBIGUITY - self._multiTagFallbackStrategy = strategy - - @property - def referencePose(self) -> Optional[Pose3d]: - """Return the reference position that is being used by the estimator. - - :returns: the referencePose - """ - return self._referencePose - - @referencePose.setter - def referencePose(self, referencePose: Pose3d | Pose2d): - """Update the stored reference pose for use when using the **CLOSEST_TO_REFERENCE_POSE** - strategy. - - :param referencePose: the referencePose to set - """ - if isinstance(referencePose, Pose2d): - referencePose = Pose3d(referencePose) - self._checkUpdate(self._referencePose, referencePose) - self._referencePose = referencePose - - @property - def lastPose(self) -> Optional[Pose3d]: - return self._lastPose - - @lastPose.setter - def lastPose(self, lastPose: Pose3d | Pose2d): - """Update the stored last pose. Useful for setting the initial estimate when using the - **CLOSEST_TO_LAST_POSE** strategy. - - :param lastPose: the lastPose to set - """ - if isinstance(lastPose, Pose2d): - lastPose = Pose3d(lastPose) - self._checkUpdate(self._lastPose, lastPose) - self._lastPose = lastPose - - def _invalidatePoseCache(self) -> None: - self._poseCacheTimestampSeconds = -1.0 - - def _checkUpdate(self, oldObj, newObj) -> None: - if oldObj != newObj and oldObj is not None and oldObj is not newObj: - self._invalidatePoseCache() - - def update( - self, cameraResult: Optional[PhotonPipelineResult] = None - ) -> Optional[EstimatedRobotPose]: - """ - Updates the estimated position of the robot. Returns empty if: - - - The timestamp of the provided pipeline result is the same as in the previous call to - ``update()``. - - - No targets were found in the pipeline results. - - :param cameraResult: The latest pipeline result from the camera - - :returns: an :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used to - create the estimate. - """ - if not cameraResult: - if not self._camera: - wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False) - return None - cameraResult = self._camera.getLatestResult() - - if cameraResult.getTimestampSeconds() < 0: - return None - - # If the pose cache timestamp was set, and the result is from the same - # timestamp, return an - # empty result - if ( - self._poseCacheTimestampSeconds > 0.0 - and abs( - self._poseCacheTimestampSeconds - cameraResult.getTimestampSeconds() - ) - < 1e-6 - ): - return None - - # Remember the timestamp of the current result used - self._poseCacheTimestampSeconds = cameraResult.getTimestampSeconds() - - # If no targets seen, trivial case -- return empty result - if not cameraResult.targets: - return None - - return self._update(cameraResult, self._primaryStrategy) - - def _update( - self, cameraResult: PhotonPipelineResult, strat: PoseStrategy - ) -> Optional[EstimatedRobotPose]: - if strat is PoseStrategy.LOWEST_AMBIGUITY: - estimatedPose = self._lowestAmbiguityStrategy(cameraResult) - elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = self._multiTagOnCoprocStrategy(cameraResult) - else: - wpilib.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False - ) - return None - - if not estimatedPose: - self._lastPose = None - - return estimatedPose - - def _multiTagOnCoprocStrategy( - self, result: PhotonPipelineResult - ) -> Optional[EstimatedRobotPose]: - if result.multiTagResult.estimatedPose.isPresent: - best_tf = result.multiTagResult.estimatedPose.best - best = ( - Pose3d() - .transformBy(best_tf) # field-to-camera - .relativeTo(self._fieldTags.getOrigin()) - .transformBy(self.robotToCamera.inverse()) # field-to-robot - ) - return EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.targets, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - ) - else: - return self._update(result, self._multiTagFallbackStrategy) - - def _lowestAmbiguityStrategy( - self, result: PhotonPipelineResult - ) -> Optional[EstimatedRobotPose]: - """ - Return the estimated position of the robot with the lowest position ambiguity from a List of - pipeline results. - - :param result: pipeline result - - :returns: the estimated position of the robot in the FCS and the estimated timestamp of this - estimation. - """ - lowestAmbiguityTarget = None - - lowestAmbiguityScore = 10.0 - for target in result.targets: - targetPoseAmbiguity = target.poseAmbiguity - - # Make sure the target is a Fiducial target. - if targetPoseAmbiguity != -1 and targetPoseAmbiguity < lowestAmbiguityScore: - lowestAmbiguityScore = targetPoseAmbiguity - lowestAmbiguityTarget = target - - # Although there are confirmed to be targets, none of them may be fiducial - # targets. - if not lowestAmbiguityTarget: - return None - - targetFiducialId = lowestAmbiguityTarget.fiducialId - - targetPosition = self._fieldTags.getTagPose(targetFiducialId) - - if not targetPosition: - self._reportFiducialPoseError(targetFiducialId) - return None - - return EstimatedRobotPose( - targetPosition.transformBy( - lowestAmbiguityTarget.getBestCameraToTarget().inverse() - ).transformBy(self.robotToCamera.inverse()), - result.getTimestampSeconds(), - result.targets, - PoseStrategy.LOWEST_AMBIGUITY, - ) - - def _reportFiducialPoseError(self, fiducialId: int) -> None: - if fiducialId not in self._reportedErrors: - wpilib.reportError( - f"[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: {fiducialId}", - False, - ) - self._reportedErrors.add(fiducialId) diff --git a/photon-lib/py/photonlibpy/targeting/TargetCorner.py b/photon-lib/py/photonlibpy/targeting/TargetCorner.py deleted file mode 100644 index de58922c2..000000000 --- a/photon-lib/py/photonlibpy/targeting/TargetCorner.py +++ /dev/null @@ -1,9 +0,0 @@ -from dataclasses import dataclass - - -@dataclass -class TargetCorner: - x: float = 0 - y: float = 9 - - photonStruct: "TargetCornerSerde" = None diff --git a/photon-lib/py/photonlibpy/targeting/__init__.py b/photon-lib/py/photonlibpy/targeting/__init__.py deleted file mode 100644 index 11360717e..000000000 --- a/photon-lib/py/photonlibpy/targeting/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -# 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 diff --git a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py deleted file mode 100644 index 6eb62d455..000000000 --- a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py +++ /dev/null @@ -1,34 +0,0 @@ -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 diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py deleted file mode 100644 index e9d5c7ca0..000000000 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ /dev/null @@ -1,65 +0,0 @@ -from dataclasses import dataclass, field -from typing import Optional - -from .multiTargetPNPResult import MultiTargetPNPResult -from .photonTrackedTarget import PhotonTrackedTarget - - -@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 - - 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 - ntReceiveTimestampMicros: int = -1 - - targets: list[PhotonTrackedTarget] = field(default_factory=list) - 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 - return ( - 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 - 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: "PhotonPipelineResultSerde" = None diff --git a/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py b/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py deleted file mode 100644 index b9204c829..000000000 --- a/photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py +++ /dev/null @@ -1,58 +0,0 @@ -from dataclasses import dataclass, field -from wpimath.geometry import Transform3d -from ..packet import Packet -from .TargetCorner import TargetCorner - - -@dataclass -class PhotonTrackedTarget: - yaw: float = 0.0 - pitch: float = 0.0 - area: float = 0.0 - skew: float = 0.0 - fiducialId: int = -1 - bestCameraToTarget: Transform3d = field(default_factory=Transform3d) - altCameraToTarget: Transform3d = field(default_factory=Transform3d) - minAreaRectCorners: list[TargetCorner] | None = None - detectedCorners: list[TargetCorner] | None = None - poseAmbiguity: float = 0.0 - - def getYaw(self) -> float: - return self.yaw - - def getPitch(self) -> float: - return self.pitch - - def getArea(self) -> float: - return self.area - - def getSkew(self) -> float: - return self.skew - - def getFiducialId(self) -> int: - return self.fiducialId - - def getPoseAmbiguity(self) -> float: - return self.poseAmbiguity - - def getMinAreaRectCorners(self) -> list[TargetCorner] | None: - return self.minAreaRectCorners - - def getDetectedCorners(self) -> list[TargetCorner] | None: - return self.detectedCorners - - def getBestCameraToTarget(self) -> Transform3d: - return self.bestCameraToTarget - - def getAlternateCameraToTarget(self) -> Transform3d: - return self.altCameraToTarget - - def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]: - retList = [] - for _ in range(numTargets): - cx = packet.decodeDouble() - cy = packet.decodeDouble() - retList.append(TargetCorner(cx, cy)) - return retList - - photonStruct: "PhotonTrackedTargetSerde" = None diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py deleted file mode 100644 index e761f5bb9..000000000 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ /dev/null @@ -1,261 +0,0 @@ -############################################################################### -## 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 . -############################################################################### - -# 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 - -# 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) - -# fieldLength = 54 / 3.281 # 54 ft -> meters -# fieldWidth = 27 / 3.281 # 24 ft -> meters - -# return AprilTagFieldLayout(tagList, fieldLength, fieldWidth) - - -# def test_lowestAmbiguityStrategy(): -# aprilTags = setupCommon() - -# 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() -# ) - -# 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) - - -# 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(), -# ) - -# 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) - - -# def test_cacheIsInvalidated(): -# aprilTags = setupCommon() - -# 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() -# ) - -# # 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) - -# # 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) - - -# def assertEquals(expected, actual, epsilon=0.0): -# assert abs(expected - actual) <= epsilon diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py index dddce605f..d50982a97 100644 --- a/photon-lib/py/test/photonlibpy_test.py +++ b/photon-lib/py/test/photonlibpy_test.py @@ -32,9 +32,8 @@ def test_roundTrip(): for i in range(5): sleep(0.1) - result = camera.getLatestResult() + result = camera.GetLatestResult() print(result) - print(camera._rawBytesEntry.getTopic().getProperties()) if __name__ == "__main__":