photonlibpy 2024.3.1__py3-none-any.whl → 2025.0.0b1__py3-none-any.whl
Sign up to get free protection for your applications and to get access to all the features.
- photonlibpy/__init__.py +21 -1
- photonlibpy/estimatedRobotPose.py +18 -1
- photonlibpy/generated/MultiTargetPNPResultSerde.py +57 -0
- photonlibpy/generated/PhotonPipelineMetadataSerde.py +69 -0
- photonlibpy/generated/PhotonPipelineResultSerde.py +65 -0
- photonlibpy/generated/PhotonTrackedTargetSerde.py +113 -0
- photonlibpy/generated/PnpResultSerde.py +71 -0
- photonlibpy/generated/TargetCornerSerde.py +57 -0
- photonlibpy/generated/__init__.py +9 -0
- photonlibpy/packet.py +175 -11
- photonlibpy/photonCamera.py +90 -15
- photonlibpy/photonPoseEstimator.py +28 -9
- photonlibpy/targeting/TargetCorner.py +9 -0
- photonlibpy/targeting/__init__.py +6 -0
- photonlibpy/targeting/multiTargetPNPResult.py +34 -0
- photonlibpy/targeting/photonPipelineResult.py +69 -0
- photonlibpy/{photonTrackedTarget.py → targeting/photonTrackedTarget.py} +9 -31
- photonlibpy/version.py +2 -2
- photonlibpy-2025.0.0b1.dist-info/METADATA +13 -0
- photonlibpy-2025.0.0b1.dist-info/RECORD +22 -0
- {photonlibpy-2024.3.1.dist-info → photonlibpy-2025.0.0b1.dist-info}/WHEEL +1 -1
- photonlibpy/multiTargetPNPResult.py +0 -49
- photonlibpy/photonPipelineResult.py +0 -43
- photonlibpy-2024.3.1.dist-info/METADATA +0 -13
- photonlibpy-2024.3.1.dist-info/RECORD +0 -13
- {photonlibpy-2024.3.1.dist-info → photonlibpy-2025.0.0b1.dist-info}/top_level.txt +0 -0
photonlibpy/packet.py
CHANGED
@@ -1,10 +1,28 @@
|
|
1
|
+
###############################################################################
|
2
|
+
## Copyright (C) Photon Vision.
|
3
|
+
###############################################################################
|
4
|
+
## This program is free software: you can redistribute it and/or modify
|
5
|
+
## it under the terms of the GNU General Public License as published by
|
6
|
+
## the Free Software Foundation, either version 3 of the License, or
|
7
|
+
## (at your option) any later version.
|
8
|
+
##
|
9
|
+
## This program is distributed in the hope that it will be useful,
|
10
|
+
## but WITHOUT ANY WARRANTY; without even the implied warranty of
|
11
|
+
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
12
|
+
## GNU General Public License for more details.
|
13
|
+
##
|
14
|
+
## You should have received a copy of the GNU General Public License
|
15
|
+
## along with this program. If not, see <https://www.gnu.org/licenses/>.
|
16
|
+
###############################################################################
|
17
|
+
|
1
18
|
import struct
|
19
|
+
from typing import Any, Optional, Type
|
2
20
|
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
|
3
21
|
import wpilib
|
4
22
|
|
5
23
|
|
6
24
|
class Packet:
|
7
|
-
def __init__(self, data: bytes):
|
25
|
+
def __init__(self, data: bytes = b""):
|
8
26
|
"""
|
9
27
|
* Constructs an empty packet.
|
10
28
|
*
|
@@ -67,7 +85,9 @@ class Packet:
|
|
67
85
|
for _ in range(numBytes):
|
68
86
|
intList.append(self._getNextByteAsInt())
|
69
87
|
|
70
|
-
# Interpret the bytes as
|
88
|
+
# Interpret the bytes as the requested type.
|
89
|
+
# Note due to NT's byte order assumptions,
|
90
|
+
# we have to flip the order of intList
|
71
91
|
value = struct.unpack(unpackFormat, bytes(intList))[0]
|
72
92
|
|
73
93
|
return value
|
@@ -78,23 +98,39 @@ class Packet:
|
|
78
98
|
*
|
79
99
|
* @return A decoded byte from the packet.
|
80
100
|
"""
|
81
|
-
return self._decodeGeneric("
|
101
|
+
return self._decodeGeneric("<b", 1)
|
82
102
|
|
83
103
|
def decode16(self) -> int:
|
84
104
|
"""
|
85
|
-
* Returns a single decoded
|
105
|
+
* Returns a single decoded short from the packet.
|
86
106
|
*
|
87
|
-
* @return A decoded
|
107
|
+
* @return A decoded short from the packet.
|
88
108
|
"""
|
89
|
-
return self._decodeGeneric("
|
109
|
+
return self._decodeGeneric("<h", 2)
|
90
110
|
|
91
|
-
def
|
111
|
+
def decodeInt(self) -> int:
|
92
112
|
"""
|
93
113
|
* Returns a decoded int (32 bytes) from the packet.
|
94
114
|
*
|
95
115
|
* @return A decoded int from the packet.
|
96
116
|
"""
|
97
|
-
return self._decodeGeneric("
|
117
|
+
return self._decodeGeneric("<l", 4)
|
118
|
+
|
119
|
+
def decodeFloat(self) -> float:
|
120
|
+
"""
|
121
|
+
* Returns a decoded float from the packet.
|
122
|
+
*
|
123
|
+
* @return A decoded float from the packet.
|
124
|
+
"""
|
125
|
+
return self._decodeGeneric("<f", 4)
|
126
|
+
|
127
|
+
def decodeLong(self) -> int:
|
128
|
+
"""
|
129
|
+
* Returns a decoded int64 from the packet.
|
130
|
+
*
|
131
|
+
* @return A decoded int64 from the packet.
|
132
|
+
"""
|
133
|
+
return self._decodeGeneric("<q", 8)
|
98
134
|
|
99
135
|
def decodeDouble(self) -> float:
|
100
136
|
"""
|
@@ -102,7 +138,7 @@ class Packet:
|
|
102
138
|
*
|
103
139
|
* @return A decoded double from the packet.
|
104
140
|
"""
|
105
|
-
return self._decodeGeneric("
|
141
|
+
return self._decodeGeneric("<d", 8)
|
106
142
|
|
107
143
|
def decodeBoolean(self) -> bool:
|
108
144
|
"""
|
@@ -115,14 +151,22 @@ class Packet:
|
|
115
151
|
def decodeDoubleArray(self, length: int) -> list[float]:
|
116
152
|
"""
|
117
153
|
* Returns a decoded array of floats from the packet.
|
118
|
-
*
|
119
|
-
* @return A decoded array of floats from the packet.
|
120
154
|
"""
|
121
155
|
ret = []
|
122
156
|
for _ in range(length):
|
123
157
|
ret.append(self.decodeDouble())
|
124
158
|
return ret
|
125
159
|
|
160
|
+
def decodeShortList(self) -> list[float]:
|
161
|
+
"""
|
162
|
+
* Returns a decoded array of shorts from the packet.
|
163
|
+
"""
|
164
|
+
length = self.decode8()
|
165
|
+
ret = []
|
166
|
+
for _ in range(length):
|
167
|
+
ret.append(self.decode16())
|
168
|
+
return ret
|
169
|
+
|
126
170
|
def decodeTransform(self) -> Transform3d:
|
127
171
|
"""
|
128
172
|
* Returns a decoded Transform3d
|
@@ -141,3 +185,123 @@ class Packet:
|
|
141
185
|
rotation = Rotation3d(Quaternion(w, x, y, z))
|
142
186
|
|
143
187
|
return Transform3d(translation, rotation)
|
188
|
+
|
189
|
+
def decodeList(self, serde: Type):
|
190
|
+
retList = []
|
191
|
+
arr_len = self.decode8()
|
192
|
+
for _ in range(arr_len):
|
193
|
+
retList.append(serde.unpack(self))
|
194
|
+
return retList
|
195
|
+
|
196
|
+
def decodeOptional(self, serde: Type) -> Optional[Any]:
|
197
|
+
if self.decodeBoolean():
|
198
|
+
return serde.unpack(self)
|
199
|
+
else:
|
200
|
+
return None
|
201
|
+
|
202
|
+
def _encodeGeneric(self, packFormat, value):
|
203
|
+
"""
|
204
|
+
Append bytes to the packet data buffer.
|
205
|
+
"""
|
206
|
+
self.packetData = self.packetData + struct.pack(packFormat, value)
|
207
|
+
self.size = len(self.packetData)
|
208
|
+
|
209
|
+
def encode8(self, value: int):
|
210
|
+
"""
|
211
|
+
Encodes a single byte and appends it to the packet.
|
212
|
+
"""
|
213
|
+
self._encodeGeneric("<b", value)
|
214
|
+
|
215
|
+
def encode16(self, value: int):
|
216
|
+
"""
|
217
|
+
Encodes a short (2 bytes) and appends it to the packet.
|
218
|
+
"""
|
219
|
+
self._encodeGeneric("<h", value)
|
220
|
+
|
221
|
+
def encodeInt(self, value: int):
|
222
|
+
"""
|
223
|
+
Encodes an int (4 bytes) and appends it to the packet.
|
224
|
+
"""
|
225
|
+
self._encodeGeneric("<l", value)
|
226
|
+
|
227
|
+
def encodeFloat(self, value: float):
|
228
|
+
"""
|
229
|
+
Encodes a float (4 bytes) and appends it to the packet.
|
230
|
+
"""
|
231
|
+
self._encodeGeneric("<f", value)
|
232
|
+
|
233
|
+
def encodeLong(self, value: int):
|
234
|
+
"""
|
235
|
+
Encodes a long (8 bytes) and appends it to the packet.
|
236
|
+
"""
|
237
|
+
self._encodeGeneric("<q", value)
|
238
|
+
|
239
|
+
def encodeDouble(self, value: float):
|
240
|
+
"""
|
241
|
+
Encodes a double (8 bytes) and appends it to the packet.
|
242
|
+
"""
|
243
|
+
self._encodeGeneric("<d", value)
|
244
|
+
|
245
|
+
def encodeBoolean(self, value: bool):
|
246
|
+
"""
|
247
|
+
Encodes a boolean as a single byte and appends it to the packet.
|
248
|
+
"""
|
249
|
+
self.encode8(1 if value else 0)
|
250
|
+
|
251
|
+
def encodeDoubleArray(self, values: list[float]):
|
252
|
+
"""
|
253
|
+
Encodes an array of doubles and appends it to the packet.
|
254
|
+
"""
|
255
|
+
self.encode8(len(values))
|
256
|
+
for value in values:
|
257
|
+
self.encodeDouble(value)
|
258
|
+
|
259
|
+
def encodeShortList(self, values: list[int]):
|
260
|
+
"""
|
261
|
+
Encodes a list of shorts, with length prefixed as a single byte.
|
262
|
+
"""
|
263
|
+
self.encode8(len(values))
|
264
|
+
for value in values:
|
265
|
+
self.encode16(value)
|
266
|
+
|
267
|
+
def encodeTransform(self, transform: Transform3d):
|
268
|
+
"""
|
269
|
+
Encodes a Transform3d (translation and rotation) and appends it to the packet.
|
270
|
+
"""
|
271
|
+
# Encode Translation3d part (x, y, z)
|
272
|
+
self.encodeDouble(transform.translation().x)
|
273
|
+
self.encodeDouble(transform.translation().y)
|
274
|
+
self.encodeDouble(transform.translation().z)
|
275
|
+
|
276
|
+
# Encode Rotation3d as Quaternion (w, x, y, z)
|
277
|
+
quaternion = transform.rotation().getQuaternion()
|
278
|
+
self.encodeDouble(quaternion.W())
|
279
|
+
self.encodeDouble(quaternion.X())
|
280
|
+
self.encodeDouble(quaternion.Y())
|
281
|
+
self.encodeDouble(quaternion.Z())
|
282
|
+
|
283
|
+
def encodeList(self, values: list[Any], serde: Type):
|
284
|
+
"""
|
285
|
+
Encodes a list of items using a specific serializer and appends it to the packet.
|
286
|
+
"""
|
287
|
+
self.encode8(len(values))
|
288
|
+
for item in values:
|
289
|
+
packed = serde.pack(item)
|
290
|
+
self.packetData = self.packetData + packed.getData()
|
291
|
+
self.size = len(self.packetData)
|
292
|
+
|
293
|
+
def encodeOptional(self, value: Optional[Any], serde: Type):
|
294
|
+
"""
|
295
|
+
Encodes an optional value using a specific serializer.
|
296
|
+
"""
|
297
|
+
if value is None:
|
298
|
+
self.encodeBoolean(False)
|
299
|
+
else:
|
300
|
+
self.encodeBoolean(True)
|
301
|
+
packed = serde.pack(value)
|
302
|
+
self.packetData = self.packetData + packed.getData()
|
303
|
+
self.size = len(self.packetData)
|
304
|
+
|
305
|
+
def encodeBytes(self, value: bytes):
|
306
|
+
self.packetData = self.packetData + value
|
307
|
+
self.size = len(self.packetData)
|
photonlibpy/photonCamera.py
CHANGED
@@ -1,10 +1,31 @@
|
|
1
|
+
###############################################################################
|
2
|
+
## Copyright (C) Photon Vision.
|
3
|
+
###############################################################################
|
4
|
+
## This program is free software: you can redistribute it and/or modify
|
5
|
+
## it under the terms of the GNU General Public License as published by
|
6
|
+
## the Free Software Foundation, either version 3 of the License, or
|
7
|
+
## (at your option) any later version.
|
8
|
+
##
|
9
|
+
## This program is distributed in the hope that it will be useful,
|
10
|
+
## but WITHOUT ANY WARRANTY; without even the implied warranty of
|
11
|
+
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
12
|
+
## GNU General Public License for more details.
|
13
|
+
##
|
14
|
+
## You should have received a copy of the GNU General Public License
|
15
|
+
## along with this program. If not, see <https://www.gnu.org/licenses/>.
|
16
|
+
###############################################################################
|
17
|
+
|
1
18
|
from enum import Enum
|
19
|
+
from typing import List
|
2
20
|
import ntcore
|
3
|
-
from wpilib import Timer
|
21
|
+
from wpilib import RobotController, Timer
|
4
22
|
import wpilib
|
5
|
-
from
|
6
|
-
from
|
7
|
-
from
|
23
|
+
from .packet import Packet
|
24
|
+
from .targeting.photonPipelineResult import PhotonPipelineResult
|
25
|
+
from .version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
|
26
|
+
|
27
|
+
# magical import to make serde stuff work
|
28
|
+
import photonlibpy.generated # noqa
|
8
29
|
|
9
30
|
|
10
31
|
class VisionLEDMode(Enum):
|
@@ -32,7 +53,9 @@ class PhotonCamera:
|
|
32
53
|
self._cameraTable = photonvision_root_table.getSubTable(cameraName)
|
33
54
|
self._path = self._cameraTable.getPath()
|
34
55
|
self._rawBytesEntry = self._cameraTable.getRawTopic("rawBytes").subscribe(
|
35
|
-
"
|
56
|
+
f"photonstruct:PhotonPipelineResult:{PhotonPipelineResult.photonStruct.MESSAGE_VERSION}",
|
57
|
+
bytes([]),
|
58
|
+
ntcore.PubSubOptions(periodic=0.01, sendAll=True),
|
36
59
|
)
|
37
60
|
|
38
61
|
self._driverModePublisher = self._cameraTable.getBooleanTopic(
|
@@ -75,23 +98,52 @@ class PhotonCamera:
|
|
75
98
|
self._prevHeartbeat = 0
|
76
99
|
self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
|
77
100
|
|
101
|
+
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
|
102
|
+
"""
|
103
|
+
The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
|
104
|
+
Calling this function clears the internal FIFO queue, and multiple calls to
|
105
|
+
getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
|
106
|
+
call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
|
107
|
+
make sure to call this frequently enough to avoid old results being discarded, too!
|
108
|
+
"""
|
109
|
+
|
110
|
+
self._versionCheck()
|
111
|
+
|
112
|
+
changes = self._rawBytesEntry.readQueue()
|
113
|
+
|
114
|
+
ret = []
|
115
|
+
|
116
|
+
for change in changes:
|
117
|
+
byteList = change.value
|
118
|
+
timestamp = change.time
|
119
|
+
|
120
|
+
if len(byteList) < 1:
|
121
|
+
pass
|
122
|
+
else:
|
123
|
+
newResult = PhotonPipelineResult()
|
124
|
+
pkt = Packet(byteList)
|
125
|
+
newResult = PhotonPipelineResult.photonStruct.unpack(pkt)
|
126
|
+
# NT4 allows us to correct the timestamp based on when the message was sent
|
127
|
+
newResult.ntReceiveTimestampMicros = timestamp
|
128
|
+
ret.append(newResult)
|
129
|
+
|
130
|
+
return ret
|
131
|
+
|
78
132
|
def getLatestResult(self) -> PhotonPipelineResult:
|
79
133
|
self._versionCheck()
|
80
134
|
|
81
|
-
|
135
|
+
now = RobotController.getFPGATime()
|
82
136
|
packetWithTimestamp = self._rawBytesEntry.getAtomic()
|
83
137
|
byteList = packetWithTimestamp.value
|
84
|
-
|
138
|
+
packetWithTimestamp.time
|
85
139
|
|
86
140
|
if len(byteList) < 1:
|
87
|
-
return
|
141
|
+
return PhotonPipelineResult()
|
88
142
|
else:
|
89
143
|
pkt = Packet(byteList)
|
90
|
-
retVal.
|
91
|
-
#
|
92
|
-
retVal.
|
93
|
-
timestamp / 1e6 - retVal.getLatencyMillis() / 1e3
|
94
|
-
)
|
144
|
+
retVal = PhotonPipelineResult.photonStruct.unpack(pkt)
|
145
|
+
# We don't trust NT4 time, hack around
|
146
|
+
retVal.ntReceiveTimestampMicros = now
|
95
147
|
return retVal
|
96
148
|
|
97
149
|
def getDriverMode(self) -> bool:
|
@@ -147,6 +199,16 @@ class PhotonCamera:
|
|
147
199
|
cameraNames = (
|
148
200
|
self._cameraTable.getInstance().getTable(self._tableName).getSubTables()
|
149
201
|
)
|
202
|
+
# Look for only cameras with rawBytes entry that exists
|
203
|
+
cameraNames = list(
|
204
|
+
filter(
|
205
|
+
lambda it: self._cameraTable.getSubTable(it)
|
206
|
+
.getEntry("rawBytes")
|
207
|
+
.exists(),
|
208
|
+
cameraNames,
|
209
|
+
)
|
210
|
+
)
|
211
|
+
|
150
212
|
if len(cameraNames) == 0:
|
151
213
|
wpilib.reportError(
|
152
214
|
"Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
|
@@ -165,7 +227,20 @@ class PhotonCamera:
|
|
165
227
|
)
|
166
228
|
|
167
229
|
versionString = self.versionEntry.get(defaultValue="")
|
168
|
-
|
230
|
+
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
|
231
|
+
|
232
|
+
remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
|
233
|
+
|
234
|
+
if remoteUUID is None or len(remoteUUID) == 0:
|
235
|
+
wpilib.reportWarning(
|
236
|
+
f"PhotonVision coprocessor at path {self._path} has not reported a message interface UUID - is your coprocessor's camera started?",
|
237
|
+
True,
|
238
|
+
)
|
239
|
+
|
240
|
+
# ntcore hands us a JSON string with leading/trailing quotes - remove those
|
241
|
+
remoteUUID = remoteUUID.replace('"', "")
|
242
|
+
|
243
|
+
if localUUID != remoteUUID:
|
169
244
|
# Verified version mismatch
|
170
245
|
|
171
246
|
bfw = """
|
@@ -190,6 +265,6 @@ class PhotonCamera:
|
|
190
265
|
|
191
266
|
wpilib.reportWarning(bfw)
|
192
267
|
|
193
|
-
errText = f"
|
268
|
+
errText = f"Photonlibpy version {PHOTONLIB_VERSION} (With message UUID {localUUID}) does not match coprocessor version {versionString} (with message UUID {remoteUUID}). Please install photonlibpy version {versionString}, or update your coprocessor to {PHOTONLIB_VERSION}."
|
194
269
|
wpilib.reportError(errText, True)
|
195
270
|
raise Exception(errText)
|
@@ -1,3 +1,20 @@
|
|
1
|
+
###############################################################################
|
2
|
+
## Copyright (C) Photon Vision.
|
3
|
+
###############################################################################
|
4
|
+
## This program is free software: you can redistribute it and/or modify
|
5
|
+
## it under the terms of the GNU General Public License as published by
|
6
|
+
## the Free Software Foundation, either version 3 of the License, or
|
7
|
+
## (at your option) any later version.
|
8
|
+
##
|
9
|
+
## This program is distributed in the hope that it will be useful,
|
10
|
+
## but WITHOUT ANY WARRANTY; without even the implied warranty of
|
11
|
+
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
12
|
+
## GNU General Public License for more details.
|
13
|
+
##
|
14
|
+
## You should have received a copy of the GNU General Public License
|
15
|
+
## along with this program. If not, see <https://www.gnu.org/licenses/>.
|
16
|
+
###############################################################################
|
17
|
+
|
1
18
|
import enum
|
2
19
|
from typing import Optional
|
3
20
|
|
@@ -5,7 +22,7 @@ import wpilib
|
|
5
22
|
from robotpy_apriltag import AprilTagFieldLayout
|
6
23
|
from wpimath.geometry import Transform3d, Pose3d, Pose2d
|
7
24
|
|
8
|
-
from .photonPipelineResult import PhotonPipelineResult
|
25
|
+
from .targeting.photonPipelineResult import PhotonPipelineResult
|
9
26
|
from .photonCamera import PhotonCamera
|
10
27
|
from .estimatedRobotPose import EstimatedRobotPose
|
11
28
|
|
@@ -207,7 +224,7 @@ class PhotonPoseEstimator:
|
|
207
224
|
return None
|
208
225
|
cameraResult = self._camera.getLatestResult()
|
209
226
|
|
210
|
-
if cameraResult.
|
227
|
+
if cameraResult.getTimestampSeconds() < 0:
|
211
228
|
return None
|
212
229
|
|
213
230
|
# If the pose cache timestamp was set, and the result is from the same
|
@@ -215,12 +232,15 @@ class PhotonPoseEstimator:
|
|
215
232
|
# empty result
|
216
233
|
if (
|
217
234
|
self._poseCacheTimestampSeconds > 0.0
|
218
|
-
and abs(
|
235
|
+
and abs(
|
236
|
+
self._poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()
|
237
|
+
)
|
238
|
+
< 1e-6
|
219
239
|
):
|
220
240
|
return None
|
221
241
|
|
222
242
|
# Remember the timestamp of the current result used
|
223
|
-
self._poseCacheTimestampSeconds = cameraResult.
|
243
|
+
self._poseCacheTimestampSeconds = cameraResult.getTimestampSeconds()
|
224
244
|
|
225
245
|
# If no targets seen, trivial case -- return empty result
|
226
246
|
if not cameraResult.targets:
|
@@ -249,8 +269,8 @@ class PhotonPoseEstimator:
|
|
249
269
|
def _multiTagOnCoprocStrategy(
|
250
270
|
self, result: PhotonPipelineResult
|
251
271
|
) -> Optional[EstimatedRobotPose]:
|
252
|
-
if result.
|
253
|
-
best_tf = result.
|
272
|
+
if result.multitagResult is not None:
|
273
|
+
best_tf = result.multitagResult.estimatedPose.best
|
254
274
|
best = (
|
255
275
|
Pose3d()
|
256
276
|
.transformBy(best_tf) # field-to-camera
|
@@ -259,7 +279,7 @@ class PhotonPoseEstimator:
|
|
259
279
|
)
|
260
280
|
return EstimatedRobotPose(
|
261
281
|
best,
|
262
|
-
result.
|
282
|
+
result.getTimestampSeconds(),
|
263
283
|
result.targets,
|
264
284
|
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
265
285
|
)
|
@@ -281,7 +301,6 @@ class PhotonPoseEstimator:
|
|
281
301
|
lowestAmbiguityTarget = None
|
282
302
|
|
283
303
|
lowestAmbiguityScore = 10.0
|
284
|
-
|
285
304
|
for target in result.targets:
|
286
305
|
targetPoseAmbiguity = target.poseAmbiguity
|
287
306
|
|
@@ -307,7 +326,7 @@ class PhotonPoseEstimator:
|
|
307
326
|
targetPosition.transformBy(
|
308
327
|
lowestAmbiguityTarget.getBestCameraToTarget().inverse()
|
309
328
|
).transformBy(self.robotToCamera.inverse()),
|
310
|
-
result.
|
329
|
+
result.getTimestampSeconds(),
|
311
330
|
result.targets,
|
312
331
|
PoseStrategy.LOWEST_AMBIGUITY,
|
313
332
|
)
|
@@ -0,0 +1,6 @@
|
|
1
|
+
# no one but us chickens
|
2
|
+
|
3
|
+
from .TargetCorner import TargetCorner # noqa
|
4
|
+
from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult # noqa
|
5
|
+
from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult # noqa
|
6
|
+
from .photonTrackedTarget import PhotonTrackedTarget # noqa
|
@@ -0,0 +1,34 @@
|
|
1
|
+
from dataclasses import dataclass, field
|
2
|
+
from wpimath.geometry import Transform3d
|
3
|
+
from ..packet import Packet
|
4
|
+
|
5
|
+
|
6
|
+
@dataclass
|
7
|
+
class PnpResult:
|
8
|
+
best: Transform3d = field(default_factory=Transform3d)
|
9
|
+
alt: Transform3d = field(default_factory=Transform3d)
|
10
|
+
ambiguity: float = 0.0
|
11
|
+
bestReprojErr: float = 0.0
|
12
|
+
altReprojErr: float = 0.0
|
13
|
+
|
14
|
+
photonStruct: "PNPResultSerde" = None
|
15
|
+
|
16
|
+
|
17
|
+
@dataclass
|
18
|
+
class MultiTargetPNPResult:
|
19
|
+
_MAX_IDS = 32
|
20
|
+
|
21
|
+
estimatedPose: PnpResult = field(default_factory=PnpResult)
|
22
|
+
fiducialIDsUsed: list[int] = field(default_factory=list)
|
23
|
+
|
24
|
+
def createFromPacket(self, packet: Packet) -> Packet:
|
25
|
+
self.estimatedPose = PnpResult()
|
26
|
+
self.estimatedPose.createFromPacket(packet)
|
27
|
+
self.fiducialIDsUsed = []
|
28
|
+
for _ in range(MultiTargetPNPResult._MAX_IDS):
|
29
|
+
fidId = packet.decode16()
|
30
|
+
if fidId >= 0:
|
31
|
+
self.fiducialIDsUsed.append(fidId)
|
32
|
+
return packet
|
33
|
+
|
34
|
+
photonStruct: "MultiTargetPNPResultSerde" = None
|
@@ -0,0 +1,69 @@
|
|
1
|
+
from dataclasses import dataclass, field
|
2
|
+
from typing import Optional
|
3
|
+
|
4
|
+
from .multiTargetPNPResult import MultiTargetPNPResult
|
5
|
+
from .photonTrackedTarget import PhotonTrackedTarget
|
6
|
+
|
7
|
+
|
8
|
+
@dataclass
|
9
|
+
class PhotonPipelineMetadata:
|
10
|
+
# Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
|
11
|
+
# reported by WPIUtilJNI::now.
|
12
|
+
captureTimestampMicros: int = -1
|
13
|
+
publishTimestampMicros: int = -1
|
14
|
+
|
15
|
+
# Mirror of the heartbeat entry -- monotonically increasing
|
16
|
+
sequenceID: int = -1
|
17
|
+
|
18
|
+
timeSinceLastPong: int = -1
|
19
|
+
|
20
|
+
photonStruct: "PhotonPipelineMetadataSerde" = None
|
21
|
+
|
22
|
+
|
23
|
+
@dataclass
|
24
|
+
class PhotonPipelineResult:
|
25
|
+
# Since we don't trust NT time sync, keep track of when we got this packet into robot code
|
26
|
+
ntReceiveTimestampMicros: int = -1
|
27
|
+
|
28
|
+
targets: list[PhotonTrackedTarget] = field(default_factory=list)
|
29
|
+
# Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
|
30
|
+
# an arbitrary timebase. This is not true in C++ or Java.
|
31
|
+
metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
|
32
|
+
multitagResult: Optional[MultiTargetPNPResult] = None
|
33
|
+
|
34
|
+
def getLatencyMillis(self) -> float:
|
35
|
+
return (
|
36
|
+
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
|
37
|
+
) / 1e3
|
38
|
+
|
39
|
+
def getTimestampSeconds(self) -> float:
|
40
|
+
"""
|
41
|
+
Returns the estimated time the frame was taken, in the Received system's time base. This is
|
42
|
+
calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
|
43
|
+
timestamp, coproc timebase))
|
44
|
+
"""
|
45
|
+
# TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
|
46
|
+
return (
|
47
|
+
self.ntReceiveTimestampMicros
|
48
|
+
- (
|
49
|
+
self.metadata.publishTimestampMicros
|
50
|
+
- self.metadata.captureTimestampMicros
|
51
|
+
)
|
52
|
+
) / 1e6
|
53
|
+
|
54
|
+
def getTargets(self) -> list[PhotonTrackedTarget]:
|
55
|
+
return self.targets
|
56
|
+
|
57
|
+
def hasTargets(self) -> bool:
|
58
|
+
return len(self.targets) > 0
|
59
|
+
|
60
|
+
def getBestTarget(self) -> PhotonTrackedTarget:
|
61
|
+
"""
|
62
|
+
Returns the best target in this pipeline result. If there are no targets, this method will
|
63
|
+
return null. The best target is determined by the target sort mode in the PhotonVision UI.
|
64
|
+
"""
|
65
|
+
if not self.hasTargets():
|
66
|
+
return None
|
67
|
+
return self.getTargets()[0]
|
68
|
+
|
69
|
+
photonStruct: "PhotonPipelineResultSerde" = None
|
@@ -1,20 +1,11 @@
|
|
1
1
|
from dataclasses import dataclass, field
|
2
2
|
from wpimath.geometry import Transform3d
|
3
|
-
from
|
4
|
-
|
5
|
-
|
6
|
-
@dataclass
|
7
|
-
class TargetCorner:
|
8
|
-
x: float
|
9
|
-
y: float
|
3
|
+
from ..packet import Packet
|
4
|
+
from .TargetCorner import TargetCorner
|
10
5
|
|
11
6
|
|
12
7
|
@dataclass
|
13
8
|
class PhotonTrackedTarget:
|
14
|
-
_MAX_CORNERS = 8
|
15
|
-
_NUM_BYTES_IN_FLOAT = 8
|
16
|
-
_PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS)
|
17
|
-
|
18
9
|
yaw: float = 0.0
|
19
10
|
pitch: float = 0.0
|
20
11
|
area: float = 0.0
|
@@ -22,9 +13,11 @@ class PhotonTrackedTarget:
|
|
22
13
|
fiducialId: int = -1
|
23
14
|
bestCameraToTarget: Transform3d = field(default_factory=Transform3d)
|
24
15
|
altCameraToTarget: Transform3d = field(default_factory=Transform3d)
|
25
|
-
minAreaRectCorners: list[TargetCorner]
|
26
|
-
detectedCorners: list[TargetCorner]
|
16
|
+
minAreaRectCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
|
17
|
+
detectedCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
|
27
18
|
poseAmbiguity: float = 0.0
|
19
|
+
objDetectId: int = -1
|
20
|
+
objDetectConf: float = 0.0
|
28
21
|
|
29
22
|
def getYaw(self) -> float:
|
30
23
|
return self.yaw
|
@@ -44,10 +37,10 @@ class PhotonTrackedTarget:
|
|
44
37
|
def getPoseAmbiguity(self) -> float:
|
45
38
|
return self.poseAmbiguity
|
46
39
|
|
47
|
-
def getMinAreaRectCorners(self) -> list[TargetCorner]
|
40
|
+
def getMinAreaRectCorners(self) -> list[TargetCorner]:
|
48
41
|
return self.minAreaRectCorners
|
49
42
|
|
50
|
-
def getDetectedCorners(self) -> list[TargetCorner]
|
43
|
+
def getDetectedCorners(self) -> list[TargetCorner]:
|
51
44
|
return self.detectedCorners
|
52
45
|
|
53
46
|
def getBestCameraToTarget(self) -> Transform3d:
|
@@ -64,19 +57,4 @@ class PhotonTrackedTarget:
|
|
64
57
|
retList.append(TargetCorner(cx, cy))
|
65
58
|
return retList
|
66
59
|
|
67
|
-
|
68
|
-
self.yaw = packet.decodeDouble()
|
69
|
-
self.pitch = packet.decodeDouble()
|
70
|
-
self.area = packet.decodeDouble()
|
71
|
-
self.skew = packet.decodeDouble()
|
72
|
-
self.fiducialId = packet.decode32()
|
73
|
-
|
74
|
-
self.bestCameraToTarget = packet.decodeTransform()
|
75
|
-
self.altCameraToTarget = packet.decodeTransform()
|
76
|
-
|
77
|
-
self.poseAmbiguity = packet.decodeDouble()
|
78
|
-
|
79
|
-
self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four
|
80
|
-
numCorners = packet.decode8()
|
81
|
-
self.detectedCorners = self._decodeTargetList(packet, numCorners)
|
82
|
-
return packet
|
60
|
+
photonStruct: "PhotonTrackedTargetSerde" = None
|
photonlibpy/version.py
CHANGED
@@ -1,2 +1,2 @@
|
|
1
|
-
PHOTONLIB_VERSION="
|
2
|
-
PHOTONVISION_VERSION="
|
1
|
+
PHOTONLIB_VERSION="v2025.0.0.beta.1"
|
2
|
+
PHOTONVISION_VERSION="v2025.0.0-beta-1"
|