photonlibpy 2024.2.2__py3-none-any.whl → 2024.2.3__py3-none-any.whl
Sign up to get free protection for your applications and to get access to all the features.
- photonlibpy/estimatedRobotPose.py +1 -1
- photonlibpy/packet.py +5 -5
- photonlibpy/photonCamera.py +3 -2
- photonlibpy/photonPipelineResult.py +3 -0
- photonlibpy/photonPoseEstimator.py +15 -15
- photonlibpy/version.py +2 -2
- {photonlibpy-2024.2.2.dist-info → photonlibpy-2024.2.3.dist-info}/METADATA +1 -1
- photonlibpy-2024.2.3.dist-info/RECORD +13 -0
- photonlibpy-2024.2.2.dist-info/RECORD +0 -13
- {photonlibpy-2024.2.2.dist-info → photonlibpy-2024.2.3.dist-info}/WHEEL +0 -0
- {photonlibpy-2024.2.2.dist-info → photonlibpy-2024.2.3.dist-info}/top_level.txt +0 -0
@@ -19,7 +19,7 @@ class EstimatedRobotPose:
|
|
19
19
|
timestampSeconds: float
|
20
20
|
"""The estimated time the frame used to derive the robot pose was taken"""
|
21
21
|
|
22
|
-
targetsUsed: [PhotonTrackedTarget]
|
22
|
+
targetsUsed: list[PhotonTrackedTarget]
|
23
23
|
"""A list of the targets used to compute this pose"""
|
24
24
|
|
25
25
|
strategy: "PoseStrategy"
|
photonlibpy/packet.py
CHANGED
@@ -4,7 +4,7 @@ import wpilib
|
|
4
4
|
|
5
5
|
|
6
6
|
class Packet:
|
7
|
-
def __init__(self, data:
|
7
|
+
def __init__(self, data: bytes):
|
8
8
|
"""
|
9
9
|
* Constructs an empty packet.
|
10
10
|
*
|
@@ -30,7 +30,7 @@ class Packet:
|
|
30
30
|
matches the version of photonlib running in the robot code.
|
31
31
|
"""
|
32
32
|
|
33
|
-
def
|
33
|
+
def _getNextByteAsInt(self) -> int:
|
34
34
|
retVal = 0x00
|
35
35
|
|
36
36
|
if not self.outOfBytes:
|
@@ -43,7 +43,7 @@ class Packet:
|
|
43
43
|
|
44
44
|
return retVal
|
45
45
|
|
46
|
-
def getData(self) ->
|
46
|
+
def getData(self) -> bytes:
|
47
47
|
"""
|
48
48
|
* Returns the packet data.
|
49
49
|
*
|
@@ -51,7 +51,7 @@ class Packet:
|
|
51
51
|
"""
|
52
52
|
return self.packetData
|
53
53
|
|
54
|
-
def setData(self, data:
|
54
|
+
def setData(self, data: bytes):
|
55
55
|
"""
|
56
56
|
* Sets the packet data.
|
57
57
|
*
|
@@ -65,7 +65,7 @@ class Packet:
|
|
65
65
|
# Read ints in from the data buffer
|
66
66
|
intList = []
|
67
67
|
for _ in range(numBytes):
|
68
|
-
intList.append(self.
|
68
|
+
intList.append(self._getNextByteAsInt())
|
69
69
|
|
70
70
|
# Interpret the bytes as a floating point number
|
71
71
|
value = struct.unpack(unpackFormat, bytes(intList))[0]
|
photonlibpy/photonCamera.py
CHANGED
@@ -4,7 +4,7 @@ from wpilib import Timer
|
|
4
4
|
import wpilib
|
5
5
|
from photonlibpy.packet import Packet
|
6
6
|
from photonlibpy.photonPipelineResult import PhotonPipelineResult
|
7
|
-
from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION
|
7
|
+
from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
|
8
8
|
|
9
9
|
|
10
10
|
class VisionLEDMode(Enum):
|
@@ -86,7 +86,8 @@ class PhotonCamera:
|
|
86
86
|
if len(byteList) < 1:
|
87
87
|
return retVal
|
88
88
|
else:
|
89
|
-
|
89
|
+
pkt = Packet(byteList)
|
90
|
+
retVal.populateFromPacket(pkt)
|
90
91
|
# NT4 allows us to correct the timestamp based on when the message was sent
|
91
92
|
retVal.setTimestampSeconds(
|
92
93
|
timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3
|
@@ -75,7 +75,7 @@ class PhotonPoseEstimator:
|
|
75
75
|
|
76
76
|
self._multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY
|
77
77
|
self._reportedErrors: set[int] = set()
|
78
|
-
self._poseCacheTimestampSeconds = -1
|
78
|
+
self._poseCacheTimestampSeconds = -1.0
|
79
79
|
self._lastPose: Optional[Pose3d] = None
|
80
80
|
self._referencePose: Optional[Pose3d] = None
|
81
81
|
|
@@ -143,7 +143,7 @@ class PhotonPoseEstimator:
|
|
143
143
|
self._multiTagFallbackStrategy = strategy
|
144
144
|
|
145
145
|
@property
|
146
|
-
def referencePose(self) -> Pose3d:
|
146
|
+
def referencePose(self) -> Optional[Pose3d]:
|
147
147
|
"""Return the reference position that is being used by the estimator.
|
148
148
|
|
149
149
|
:returns: the referencePose
|
@@ -163,7 +163,7 @@ class PhotonPoseEstimator:
|
|
163
163
|
self._referencePose = referencePose
|
164
164
|
|
165
165
|
@property
|
166
|
-
def lastPose(self) -> Pose3d:
|
166
|
+
def lastPose(self) -> Optional[Pose3d]:
|
167
167
|
return self._lastPose
|
168
168
|
|
169
169
|
@lastPose.setter
|
@@ -178,10 +178,10 @@ class PhotonPoseEstimator:
|
|
178
178
|
self._checkUpdate(self._lastPose, lastPose)
|
179
179
|
self._lastPose = lastPose
|
180
180
|
|
181
|
-
def _invalidatePoseCache(self):
|
182
|
-
self._poseCacheTimestampSeconds = -1
|
181
|
+
def _invalidatePoseCache(self) -> None:
|
182
|
+
self._poseCacheTimestampSeconds = -1.0
|
183
183
|
|
184
|
-
def _checkUpdate(self, oldObj, newObj):
|
184
|
+
def _checkUpdate(self, oldObj, newObj) -> None:
|
185
185
|
if oldObj != newObj and oldObj is not None and oldObj is not newObj:
|
186
186
|
self._invalidatePoseCache()
|
187
187
|
|
@@ -204,27 +204,27 @@ class PhotonPoseEstimator:
|
|
204
204
|
if not cameraResult:
|
205
205
|
if not self._camera:
|
206
206
|
wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False)
|
207
|
-
return
|
207
|
+
return None
|
208
208
|
cameraResult = self._camera.getLatestResult()
|
209
209
|
|
210
210
|
if cameraResult.timestampSec < 0:
|
211
|
-
return
|
211
|
+
return None
|
212
212
|
|
213
213
|
# If the pose cache timestamp was set, and the result is from the same
|
214
214
|
# timestamp, return an
|
215
215
|
# empty result
|
216
216
|
if (
|
217
|
-
self._poseCacheTimestampSeconds > 0
|
217
|
+
self._poseCacheTimestampSeconds > 0.0
|
218
218
|
and abs(self._poseCacheTimestampSeconds - cameraResult.timestampSec) < 1e-6
|
219
219
|
):
|
220
|
-
return
|
220
|
+
return None
|
221
221
|
|
222
222
|
# Remember the timestamp of the current result used
|
223
223
|
self._poseCacheTimestampSeconds = cameraResult.timestampSec
|
224
224
|
|
225
225
|
# If no targets seen, trivial case -- return empty result
|
226
226
|
if not cameraResult.targets:
|
227
|
-
return
|
227
|
+
return None
|
228
228
|
|
229
229
|
return self._update(cameraResult, self._primaryStrategy)
|
230
230
|
|
@@ -239,7 +239,7 @@ class PhotonPoseEstimator:
|
|
239
239
|
wpilib.reportError(
|
240
240
|
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
|
241
241
|
)
|
242
|
-
return
|
242
|
+
return None
|
243
243
|
|
244
244
|
if not estimatedPose:
|
245
245
|
self._lastPose = None
|
@@ -280,7 +280,7 @@ class PhotonPoseEstimator:
|
|
280
280
|
"""
|
281
281
|
lowestAmbiguityTarget = None
|
282
282
|
|
283
|
-
lowestAmbiguityScore = 10
|
283
|
+
lowestAmbiguityScore = 10.0
|
284
284
|
|
285
285
|
for target in result.targets:
|
286
286
|
targetPoseAmbiguity = target.poseAmbiguity
|
@@ -293,7 +293,7 @@ class PhotonPoseEstimator:
|
|
293
293
|
# Although there are confirmed to be targets, none of them may be fiducial
|
294
294
|
# targets.
|
295
295
|
if not lowestAmbiguityTarget:
|
296
|
-
return
|
296
|
+
return None
|
297
297
|
|
298
298
|
targetFiducialId = lowestAmbiguityTarget.fiducialId
|
299
299
|
|
@@ -301,7 +301,7 @@ class PhotonPoseEstimator:
|
|
301
301
|
|
302
302
|
if not targetPosition:
|
303
303
|
self._reportFiducialPoseError(targetFiducialId)
|
304
|
-
return
|
304
|
+
return None
|
305
305
|
|
306
306
|
return EstimatedRobotPose(
|
307
307
|
targetPosition.transformBy(
|
photonlibpy/version.py
CHANGED
@@ -1,2 +1,2 @@
|
|
1
|
-
PHOTONLIB_VERSION="2024.2.
|
2
|
-
PHOTONVISION_VERSION="v2024.2.
|
1
|
+
PHOTONLIB_VERSION="2024.2.3"
|
2
|
+
PHOTONVISION_VERSION="v2024.2.3"
|
@@ -0,0 +1,13 @@
|
|
1
|
+
photonlibpy/__init__.py,sha256=Poafj5ngdQ2T0yW3zhnQ2NHw70XrTwXvT8aDCAImYbg,30
|
2
|
+
photonlibpy/estimatedRobotPose.py,sha256=_lgVu157fBpG85WpeS4uXWCF4aQDUbMI2SWXG5lee9I,673
|
3
|
+
photonlibpy/multiTargetPNPResult.py,sha256=AgrRjSna1s8yJWUdBgWxqKxm0cqgXwYrKEzgfP7pzGE,1660
|
4
|
+
photonlibpy/packet.py,sha256=Sg_Sln_DNYvdT0DpekIb6v67e3c4oX08zLQe9oDd2OU,3969
|
5
|
+
photonlibpy/photonCamera.py,sha256=hge7uIgmU7em43OaiHp3QK9BYcfEwgkos1IDAy_DqXk,6948
|
6
|
+
photonlibpy/photonPipelineResult.py,sha256=IpHxXKuyfOKcslgm_Ce2PqV8HgXTN_IMUuNb5BBTzSo,1351
|
7
|
+
photonlibpy/photonPoseEstimator.py,sha256=s99TjCi3SFpCo5fPckw2ZfJK7E3lvFO3zpr0GnDDyKM,11580
|
8
|
+
photonlibpy/photonTrackedTarget.py,sha256=Br2L4fNNwQP4eUj9eV9I653FOER00JbeHB5SMNTSaHc,2482
|
9
|
+
photonlibpy/version.py,sha256=SuU2vxVKanl1FmuE_j7rnJz-ODlNnZ3v84gd2qHn9l0,62
|
10
|
+
photonlibpy-2024.2.3.dist-info/METADATA,sha256=rM_CgwPHAD6D8-NQU_j-VoQHxCr9POKstPyJLACbPnY,500
|
11
|
+
photonlibpy-2024.2.3.dist-info/WHEEL,sha256=oiQVh_5PnQM0E3gPdiz09WCNmwiHDMaGer_elqB3coM,92
|
12
|
+
photonlibpy-2024.2.3.dist-info/top_level.txt,sha256=T8Xc6U6he2VjKUAca6zawSkHdUZuLanxYIc4nxw2ctc,12
|
13
|
+
photonlibpy-2024.2.3.dist-info/RECORD,,
|
@@ -1,13 +0,0 @@
|
|
1
|
-
photonlibpy/__init__.py,sha256=Poafj5ngdQ2T0yW3zhnQ2NHw70XrTwXvT8aDCAImYbg,30
|
2
|
-
photonlibpy/estimatedRobotPose.py,sha256=Q3uB71GD2f-HZEa27EqdK_ordwtaYAfnM8bX94xzkvQ,669
|
3
|
-
photonlibpy/multiTargetPNPResult.py,sha256=AgrRjSna1s8yJWUdBgWxqKxm0cqgXwYrKEzgfP7pzGE,1660
|
4
|
-
photonlibpy/packet.py,sha256=76LE5Cua1c6WzIdxVVBKpgRkIyujNqF1GmowJ7SqJsc,3971
|
5
|
-
photonlibpy/photonCamera.py,sha256=TZQp-wjXly6W2Ca-9u3QWlciutR9UnEf4BJDVvHpbPw,6894
|
6
|
-
photonlibpy/photonPipelineResult.py,sha256=jtQH-kfYamukbtPbpnJQWhosRXfA7Mev5dvIxw6E_Qo,1279
|
7
|
-
photonlibpy/photonPoseEstimator.py,sha256=San3JDkYOJtmqZHo9PktkuF6TIXmDZBbMlPMYjuJbXw,11501
|
8
|
-
photonlibpy/photonTrackedTarget.py,sha256=Br2L4fNNwQP4eUj9eV9I653FOER00JbeHB5SMNTSaHc,2482
|
9
|
-
photonlibpy/version.py,sha256=7tGh5KZlUTAJs2oiNnSCJoy_n1Df010CrsiUXHAjwhg,62
|
10
|
-
photonlibpy-2024.2.2.dist-info/METADATA,sha256=mgP83iatHmtAbNd2AiMXFelARMOoupjrNoTnHqvuuCU,500
|
11
|
-
photonlibpy-2024.2.2.dist-info/WHEEL,sha256=oiQVh_5PnQM0E3gPdiz09WCNmwiHDMaGer_elqB3coM,92
|
12
|
-
photonlibpy-2024.2.2.dist-info/top_level.txt,sha256=T8Xc6U6he2VjKUAca6zawSkHdUZuLanxYIc4nxw2ctc,12
|
13
|
-
photonlibpy-2024.2.2.dist-info/RECORD,,
|
File without changes
|
File without changes
|