photonlibpy 2024.3.1__py3-none-any.whl → 2025.0.0b1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- 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"
|