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/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 a floating point number
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(">b", 1)
101
+ return self._decodeGeneric("<b", 1)
82
102
 
83
103
  def decode16(self) -> int:
84
104
  """
85
- * Returns a single decoded byte from the packet.
105
+ * Returns a single decoded short from the packet.
86
106
  *
87
- * @return A decoded byte from the packet.
107
+ * @return A decoded short from the packet.
88
108
  """
89
- return self._decodeGeneric(">h", 2)
109
+ return self._decodeGeneric("<h", 2)
90
110
 
91
- def decode32(self) -> int:
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(">l", 4)
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(">d", 8)
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)
@@ -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 photonlibpy.packet import Packet
6
- from photonlibpy.photonPipelineResult import PhotonPipelineResult
7
- from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
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
- "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True)
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
- retVal = PhotonPipelineResult()
135
+ now = RobotController.getFPGATime()
82
136
  packetWithTimestamp = self._rawBytesEntry.getAtomic()
83
137
  byteList = packetWithTimestamp.value
84
- timestamp = packetWithTimestamp.time
138
+ packetWithTimestamp.time
85
139
 
86
140
  if len(byteList) < 1:
87
- return retVal
141
+ return PhotonPipelineResult()
88
142
  else:
89
143
  pkt = Packet(byteList)
90
- retVal.populateFromPacket(pkt)
91
- # NT4 allows us to correct the timestamp based on when the message was sent
92
- retVal.setTimestampSeconds(
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
- if len(versionString) > 0 and versionString != PHOTONVISION_VERSION:
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"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {PHOTONLIB_VERSION}."
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.timestampSec < 0:
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(self._poseCacheTimestampSeconds - cameraResult.timestampSec) < 1e-6
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.timestampSec
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.multiTagResult.estimatedPose.isPresent:
253
- best_tf = result.multiTagResult.estimatedPose.best
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.timestampSec,
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.timestampSec,
329
+ result.getTimestampSeconds(),
311
330
  result.targets,
312
331
  PoseStrategy.LOWEST_AMBIGUITY,
313
332
  )
@@ -0,0 +1,9 @@
1
+ from dataclasses import dataclass
2
+
3
+
4
+ @dataclass
5
+ class TargetCorner:
6
+ x: float = 0
7
+ y: float = 9
8
+
9
+ photonStruct: "TargetCornerSerde" = None
@@ -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 photonlibpy.packet import Packet
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] | None = None
26
- detectedCorners: list[TargetCorner] | None = None
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] | None:
40
+ def getMinAreaRectCorners(self) -> list[TargetCorner]:
48
41
  return self.minAreaRectCorners
49
42
 
50
- def getDetectedCorners(self) -> list[TargetCorner] | None:
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
- def createFromPacket(self, packet: Packet) -> Packet:
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="2024.3.1"
2
- PHOTONVISION_VERSION="v2024.3.1"
1
+ PHOTONLIB_VERSION="v2025.0.0.beta.1"
2
+ PHOTONVISION_VERSION="v2025.0.0-beta-1"