photonlibpy 2025.0.0b3__tar.gz → 2025.0.0b5__tar.gz

Sign up to get free protection for your applications and to get access to all the features.
Files changed (43) hide show
  1. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/PKG-INFO +2 -2
  2. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/openCVHelp.py +110 -1
  3. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/rotTrlTransform3d.py +19 -0
  4. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/targetModel.py +63 -0
  5. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/visionEstimation.py +22 -0
  6. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/PhotonPipelineResultSerde.py +1 -3
  7. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/networktables/NTTopicSet.py +7 -0
  8. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/photonCamera.py +102 -36
  9. photonlibpy-2025.0.0b5/photonlibpy/py.typed +1 -0
  10. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/photonCameraSim.py +109 -3
  11. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/simCameraProperties.py +142 -1
  12. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/visionSystemSim.py +92 -0
  13. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/visionTargetSim.py +10 -0
  14. photonlibpy-2025.0.0b5/photonlibpy/version.py +2 -0
  15. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy.egg-info/PKG-INFO +2 -2
  16. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy.egg-info/SOURCES.txt +1 -0
  17. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy.egg-info/requires.txt +0 -3
  18. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/setup.py +1 -1
  19. photonlibpy-2025.0.0b3/photonlibpy/version.py +0 -2
  20. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/__init__.py +0 -0
  21. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimatedRobotPose.py +0 -0
  22. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/__init__.py +0 -0
  23. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/estimation/cameraTargetRelation.py +0 -0
  24. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/MultiTargetPNPResultSerde.py +0 -0
  25. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/PhotonPipelineMetadataSerde.py +0 -0
  26. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/PhotonTrackedTargetSerde.py +0 -0
  27. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/PnpResultSerde.py +0 -0
  28. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/TargetCornerSerde.py +0 -0
  29. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/generated/__init__.py +0 -0
  30. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/networktables/__init__.py +0 -0
  31. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/packet.py +0 -0
  32. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/photonPoseEstimator.py +0 -0
  33. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/__init__.py +0 -0
  34. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/simulation/videoSimUtil.py +0 -0
  35. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/targeting/TargetCorner.py +0 -0
  36. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/targeting/__init__.py +0 -0
  37. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/targeting/multiTargetPNPResult.py +0 -0
  38. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/targeting/photonPipelineResult.py +0 -0
  39. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy/targeting/photonTrackedTarget.py +0 -0
  40. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy.egg-info/dependency_links.txt +0 -0
  41. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/photonlibpy.egg-info/top_level.txt +0 -0
  42. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/pyproject.toml +0 -0
  43. {photonlibpy-2025.0.0b3 → photonlibpy-2025.0.0b5}/setup.cfg +0 -0
@@ -1,7 +1,7 @@
1
1
  Metadata-Version: 2.1
2
2
  Name: photonlibpy
3
- Version: 2025.0.0b3
4
- Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.0.0-beta-3 .
3
+ Version: 2025.0.0b5
4
+ Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.0.0-beta-5 .
5
5
  Home-page: https://photonvision.org
6
6
  Author: Photonvision Development Team
7
7
  Description-Content-Type: text/markdown
@@ -1,3 +1,4 @@
1
+ import logging
1
2
  import math
2
3
  from typing import Any
3
4
 
@@ -11,6 +12,8 @@ from .rotTrlTransform3d import RotTrlTransform3d
11
12
  NWU_TO_EDN = Rotation3d(np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]))
12
13
  EDN_TO_NWU = Rotation3d(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]]))
13
14
 
15
+ logger = logging.getLogger(__name__)
16
+
14
17
 
15
18
  class OpenCVHelp:
16
19
  @staticmethod
@@ -27,6 +30,12 @@ class OpenCVHelp:
27
30
 
28
31
  @staticmethod
29
32
  def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
33
+ """Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
34
+ three elements representing {x, y, z} in the EDN coordinate system.
35
+
36
+ :param translations: The translations to convert into a np.array
37
+ """
38
+
30
39
  retVal: list[list] = []
31
40
  for translation in translations:
32
41
  trl = OpenCVHelp.translationNWUtoEDN(translation)
@@ -38,6 +47,13 @@ class OpenCVHelp:
38
47
 
39
48
  @staticmethod
40
49
  def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
50
+ """Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
51
+ three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
52
+ norm, and axis = rvec / norm)
53
+
54
+ :param rotation: The rotation to convert into a np.array
55
+ """
56
+
41
57
  retVal: list[np.ndarray] = []
42
58
  rot = OpenCVHelp.rotationNWUtoEDN(rotation)
43
59
  rotVec = rot.getQuaternion().toRotationVector()
@@ -88,6 +104,25 @@ class OpenCVHelp:
88
104
  def reorderCircular(
89
105
  elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
90
106
  ) -> list[Any]:
107
+ """Reorders the list, optionally indexing backwards and wrapping around to the last element after
108
+ the first, and shifting all indices in the direction of indexing.
109
+
110
+ e.g.
111
+
112
+ ({1,2,3}, false, 1) == {2,3,1}
113
+
114
+ ({1,2,3}, true, 0) == {1,3,2}
115
+
116
+ ({1,2,3}, true, 1) == {3,2,1}
117
+
118
+ :param elements: list elements
119
+ :param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
120
+ :param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
121
+ start at shiftStart, negated if backwards)
122
+
123
+ :returns: Reordered list
124
+ """
125
+
91
126
  size = len(elements)
92
127
  reordered = []
93
128
  dir = -1 if backwards else 1
@@ -100,18 +135,39 @@ class OpenCVHelp:
100
135
 
101
136
  @staticmethod
102
137
  def translationEDNToNWU(trl: Translation3d) -> Translation3d:
138
+ """Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
139
+ in EDN, this would be {0, -1, 0} in NWU.
140
+ """
141
+
103
142
  return trl.rotateBy(EDN_TO_NWU)
104
143
 
105
144
  @staticmethod
106
145
  def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
146
+ """Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
147
+ in NWU, this would be {0, 0, 1} in EDN.
148
+ """
149
+
107
150
  return -EDN_TO_NWU + (rot + EDN_TO_NWU)
108
151
 
109
152
  @staticmethod
110
153
  def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
154
+ """Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
155
+ elements representing {x, y, z} in the EDN coordinate system.
156
+
157
+ :param tvecInput: The tvec to create a Translation3d from
158
+ """
159
+
111
160
  return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
112
161
 
113
162
  @staticmethod
114
163
  def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
164
+ """Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
165
+ elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
166
+ and axis = rvec / norm)
167
+
168
+ :param rvecInput: The rvec to create a Rotation3d from
169
+ """
170
+
115
171
  return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
116
172
 
117
173
  @staticmethod
@@ -121,6 +177,33 @@ class OpenCVHelp:
121
177
  modelTrls: list[Translation3d],
122
178
  imagePoints: np.ndarray,
123
179
  ) -> PnpResult | None:
180
+ """Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
181
+ relative to the target is determined by the supplied 3d points of the target's model and their
182
+ associated 2d points imaged by the camera. The supplied model translations must be relative to
183
+ the target's pose.
184
+
185
+ For planar targets, there may be an alternate solution which is plausible given the 2d image
186
+ points. This has an associated "ambiguity" which describes the ratio of reprojection error
187
+ between the "best" and "alternate" solution.
188
+
189
+ This method is intended for use with individual AprilTags, and will not work unless 4 points
190
+ are provided.
191
+
192
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
193
+ :param distCoeffs: The camera distortion matrix in standard opencv form
194
+ :param modelTrls: The translations of the object corners. These should have the object pose as
195
+ their origin. These must come in a specific, pose-relative order (in NWU):
196
+
197
+ - Point 0: [0, -squareLength / 2, squareLength / 2]
198
+ - Point 1: [0, squareLength / 2, squareLength / 2]
199
+ - Point 2: [0, squareLength / 2, -squareLength / 2]
200
+ - Point 3: [0, -squareLength / 2, -squareLength / 2]
201
+ :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
202
+ should match the given object point translations.
203
+
204
+ :returns: The resulting transformation that maps the camera pose to the target pose and the
205
+ ambiguity if an alternate solution is available.
206
+ """
124
207
  modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
125
208
  imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
126
209
  objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
@@ -130,6 +213,7 @@ class OpenCVHelp:
130
213
  best: Transform3d = Transform3d()
131
214
 
132
215
  for tries in range(2):
216
+ # calc rvecs/tvecs and associated reprojection error from image points
133
217
  retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
134
218
  objectMat,
135
219
  imagePoints,
@@ -138,6 +222,7 @@ class OpenCVHelp:
138
222
  flags=cv.SOLVEPNP_IPPE_SQUARE,
139
223
  )
140
224
 
225
+ # convert to wpilib coordinates
141
226
  best = Transform3d(
142
227
  OpenCVHelp.tVecToTranslation(tvecs[0]),
143
228
  OpenCVHelp.rVecToRotation(rvecs[0]),
@@ -148,6 +233,7 @@ class OpenCVHelp:
148
233
  OpenCVHelp.rVecToRotation(rvecs[1]),
149
234
  )
150
235
 
236
+ # check if we got a NaN result
151
237
  if reprojectionError is not None and not math.isnan(
152
238
  reprojectionError[0, 0]
153
239
  ):
@@ -158,8 +244,9 @@ class OpenCVHelp:
158
244
  pt[0, 1] -= 0.001
159
245
  imagePoints[0] = pt
160
246
 
247
+ # solvePnP failed
161
248
  if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
162
- print("SolvePNP_Square failed!")
249
+ logger.error("SolvePNP_Square failed!")
163
250
  return None
164
251
 
165
252
  if alt:
@@ -186,6 +273,27 @@ class OpenCVHelp:
186
273
  modelTrls: list[Translation3d],
187
274
  imagePoints: np.ndarray,
188
275
  ) -> PnpResult | None:
276
+ """Finds the transformation that maps the camera's pose to the origin of the supplied object. An
277
+ "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
278
+ for example, the object translations are given relative to close-right corner of the blue
279
+ alliance(the default origin), a camera-to-origin transformation is returned. If the
280
+ translations are relative to a target's pose, a camera-to-target transformation is returned.
281
+
282
+ There must be at least 3 points to use this method. This does not return an alternate
283
+ solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
284
+ #solvePNP_SQUARE} instead.
285
+
286
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
287
+ :param distCoeffs: The camera distortion matrix in standard opencv form
288
+ :param objectTrls: The translations of the object corners, relative to the field.
289
+ :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
290
+ should match the given object point translations.
291
+
292
+ :returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
293
+ model points are supplied relative to the origin, this transformation brings the camera to
294
+ the origin.
295
+ """
296
+
189
297
  objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
190
298
 
191
299
  retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
@@ -198,6 +306,7 @@ class OpenCVHelp:
198
306
  )
199
307
 
200
308
  if math.isnan(error):
309
+ logger.error("SolvePNP_SQPNP failed!")
201
310
  return None
202
311
 
203
312
  # We have no alternative so set it to best as well
@@ -4,24 +4,38 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
4
4
 
5
5
 
6
6
  class RotTrlTransform3d:
7
+ """Represents a transformation that first rotates a pose around the origin, and then translates it."""
8
+
7
9
  def __init__(
8
10
  self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
9
11
  ):
12
+ """A rotation-translation transformation.
13
+
14
+ Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
15
+ transform as if the origin was transformed by these components instead.
16
+
17
+ :param rot: The rotation component
18
+ :param trl: The translation component
19
+ """
10
20
  self.rot = rot
11
21
  self.trl = trl
12
22
 
13
23
  def inverse(self) -> Self:
24
+ """The inverse of this transformation. Applying the inverse will "undo" this transformation."""
14
25
  invRot = -self.rot
15
26
  invTrl = -(self.trl.rotateBy(invRot))
16
27
  return type(self)(invRot, invTrl)
17
28
 
18
29
  def getTransform(self) -> Transform3d:
30
+ """This transformation as a Transform3d (as if of the origin)"""
19
31
  return Transform3d(self.trl, self.rot)
20
32
 
21
33
  def getTranslation(self) -> Translation3d:
34
+ """The translation component of this transformation"""
22
35
  return self.trl
23
36
 
24
37
  def getRotation(self) -> Rotation3d:
38
+ """The rotation component of this transformation"""
25
39
  return self.rot
26
40
 
27
41
  def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
44
58
 
45
59
  @classmethod
46
60
  def makeRelativeTo(cls, pose: Pose3d) -> Self:
61
+ """The rotation-translation transformation that makes poses in the world consider this pose as the
62
+ new origin, or change the basis to this pose.
63
+
64
+ :param pose: The new origin
65
+ """
47
66
  return cls(pose.rotation(), pose.translation()).inverse()
48
67
 
49
68
  @classmethod
@@ -8,14 +8,27 @@ from . import RotTrlTransform3d
8
8
 
9
9
 
10
10
  class TargetModel:
11
+ """Describes the 3d model of a target."""
11
12
 
12
13
  def __init__(self):
14
+ """Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
15
+ createCuboid, createSpheroid or create Arbitrary
16
+ """
13
17
  self.vertices: List[Translation3d] = []
14
18
  self.isPlanar = False
15
19
  self.isSpherical = False
16
20
 
17
21
  @classmethod
18
22
  def createPlanar(cls, width: meters, height: meters) -> Self:
23
+ """Creates a rectangular, planar target model given the width and height. The model has four
24
+ vertices:
25
+
26
+ - Point 0: [0, -width/2, -height/2]
27
+ - Point 1: [0, width/2, -height/2]
28
+ - Point 2: [0, width/2, height/2]
29
+ - Point 3: [0, -width/2, height/2]
30
+ """
31
+
19
32
  tm = cls()
20
33
 
21
34
  tm.isPlanar = True
@@ -30,6 +43,18 @@ class TargetModel:
30
43
 
31
44
  @classmethod
32
45
  def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
46
+ """Creates a cuboid target model given the length, width, height. The model has eight vertices:
47
+
48
+ - Point 0: [length/2, -width/2, -height/2]
49
+ - Point 1: [length/2, width/2, -height/2]
50
+ - Point 2: [length/2, width/2, height/2]
51
+ - Point 3: [length/2, -width/2, height/2]
52
+ - Point 4: [-length/2, -width/2, height/2]
53
+ - Point 5: [-length/2, width/2, height/2]
54
+ - Point 6: [-length/2, width/2, -height/2]
55
+ - Point 7: [-length/2, -width/2, -height/2]
56
+ """
57
+
33
58
  tm = cls()
34
59
  verts = [
35
60
  Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
@@ -48,6 +73,20 @@ class TargetModel:
48
73
 
49
74
  @classmethod
50
75
  def createSpheroid(cls, diameter: meters) -> Self:
76
+ """Creates a spherical target model which has similar dimensions regardless of its rotation. This
77
+ model has four vertices:
78
+
79
+ - Point 0: [0, -radius, 0]
80
+ - Point 1: [0, 0, -radius]
81
+ - Point 2: [0, radius, 0]
82
+ - Point 3: [0, 0, radius]
83
+
84
+ *Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
85
+ like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
86
+ are used for drawing the image of this sphere, but do not match the corners that will be
87
+ published by photonvision.
88
+ """
89
+
51
90
  tm = cls()
52
91
 
53
92
  tm.isPlanar = False
@@ -63,6 +102,14 @@ class TargetModel:
63
102
 
64
103
  @classmethod
65
104
  def createArbitrary(cls, verts: List[Translation3d]) -> Self:
105
+ """Creates a target model from arbitrary 3d vertices. Automatically determines if the given
106
+ vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
107
+ vertices should define a non-intersecting contour.
108
+
109
+ :param vertices: Translations representing the vertices of this target model relative to its
110
+ pose.
111
+ """
112
+
66
113
  tm = cls()
67
114
  tm._common_construction(verts)
68
115
 
@@ -83,6 +130,12 @@ class TargetModel:
83
130
  self.vertices = verts
84
131
 
85
132
  def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
133
+ """This target's vertices offset from its field pose.
134
+
135
+ Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
136
+ Translation3d)} with this method.
137
+ """
138
+
86
139
  basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
87
140
 
88
141
  retVal = []
@@ -94,6 +147,16 @@ class TargetModel:
94
147
 
95
148
  @classmethod
96
149
  def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
150
+ """Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
151
+ to the camera translation. This is used for spherical targets which should not have their
152
+ projection change regardless of their own rotation.
153
+
154
+ :param tgtTrl: This target's translation
155
+ :param cameraTrl: Camera's translation
156
+
157
+ :returns: This target's pose oriented to the camera
158
+ """
159
+
97
160
  relCam = cameraTrl - tgtTrl
98
161
  orientToCam = Rotation3d(
99
162
  0.0,
@@ -11,6 +11,7 @@ class VisionEstimation:
11
11
  def getVisibleLayoutTags(
12
12
  visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
13
13
  ) -> list[AprilTag]:
14
+ """Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
14
15
  retVal: list[AprilTag] = []
15
16
  for tag in visTags:
16
17
  id = tag.getFiducialId()
@@ -30,12 +31,31 @@ class VisionEstimation:
30
31
  layout: AprilTagFieldLayout,
31
32
  tagModel: TargetModel,
32
33
  ) -> PnpResult | None:
34
+ """Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
35
+ field-to-camera transformation. If only one tag is visible, the result may have an alternate
36
+ solution.
37
+
38
+ **Note:** The returned transformation is from the field origin to the camera pose!
39
+
40
+ With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
41
+
42
+ With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
43
+
44
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
45
+ :param distCoeffs: The camera distortion matrix in standard opencv form
46
+ :param visTags: The visible tags reported by PV. Non-tag targets are automatically excluded.
47
+ :param tagLayout: The known tag layout on the field
48
+
49
+ :returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
50
+ PnpResult} are present before utilizing them.
51
+ """
33
52
  if len(visTags) == 0:
34
53
  return None
35
54
 
36
55
  corners: list[TargetCorner] = []
37
56
  knownTags: list[AprilTag] = []
38
57
 
58
+ # ensure these are AprilTags in our layout
39
59
  for tgt in visTags:
40
60
  id = tgt.getFiducialId()
41
61
  maybePose = layout.getTagPose(id)
@@ -53,6 +73,7 @@ class VisionEstimation:
53
73
 
54
74
  points = OpenCVHelp.cornersToPoints(corners)
55
75
 
76
+ # single-tag pnp
56
77
  if len(knownTags) == 1:
57
78
  camToTag = OpenCVHelp.solvePNP_Square(
58
79
  cameraMatrix, distCoeffs, tagModel.getVertices(), points
@@ -74,6 +95,7 @@ class VisionEstimation:
74
95
  altReprojErr=camToTag.altReprojErr,
75
96
  )
76
97
  return result
98
+ # multi-tag pnp
77
99
  else:
78
100
  objectTrls: list[Translation3d] = []
79
101
  for tag in knownTags:
@@ -42,9 +42,7 @@ class PhotonPipelineResultSerde:
42
42
  ret = Packet()
43
43
 
44
44
  # metadata is of non-intrinsic type PhotonPipelineMetadata
45
- ret.encodeBytes(
46
- PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
47
- )
45
+ ret.encodeBytes(PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData())
48
46
 
49
47
  # targets is a custom VLA!
50
48
  ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
@@ -9,6 +9,13 @@ PhotonPipelineResult_TYPE_STRING = (
9
9
 
10
10
 
11
11
  class NTTopicSet:
12
+ """This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
13
+ It's split here so the sim and real-camera implementations can share a common implementation of
14
+ the naming and registration of the NT content.
15
+
16
+ However, we do expect that the actual logic which fills out values in the entries will be
17
+ different for sim vs. real camera
18
+ """
12
19
 
13
20
  def __init__(self, tableName: str, cameraName: str) -> None:
14
21
  instance = nt.NetworkTableInstance.getDefault()
@@ -48,6 +48,10 @@ def setVersionCheckEnabled(enabled: bool):
48
48
 
49
49
  class PhotonCamera:
50
50
  def __init__(self, cameraName: str):
51
+ """Constructs a PhotonCamera from the name of the camera.
52
+
53
+ :param cameraName: The nickname of the camera (found in the PhotonVision UI).
54
+ """
51
55
  instance = ntcore.NetworkTableInstance.getDefault()
52
56
  self._name = cameraName
53
57
  self._tableName = "photonvision"
@@ -132,6 +136,14 @@ class PhotonCamera:
132
136
  return ret
133
137
 
134
138
  def getLatestResult(self) -> PhotonPipelineResult:
139
+ """Returns the latest pipeline result. This is simply the most recent result Received via NT.
140
+ Calling this multiple times will always return the most recent result.
141
+
142
+ Replaced by :meth:`.getAllUnreadResults` over getLatestResult, as this function can miss
143
+ results, or provide duplicate ones!
144
+ TODO implement the thing that will take this ones place...
145
+ """
146
+
135
147
  self._versionCheck()
136
148
 
137
149
  now = RobotController.getFPGATime()
@@ -149,34 +161,85 @@ class PhotonCamera:
149
161
  return retVal
150
162
 
151
163
  def getDriverMode(self) -> bool:
164
+ """Returns whether the camera is in driver mode.
165
+
166
+ :returns: Whether the camera is in driver mode.
167
+ """
168
+
152
169
  return self._driverModeSubscriber.get()
153
170
 
154
171
  def setDriverMode(self, driverMode: bool) -> None:
172
+ """Toggles driver mode.
173
+
174
+ :param driverMode: Whether to set driver mode.
175
+ """
176
+
155
177
  self._driverModePublisher.set(driverMode)
156
178
 
157
179
  def takeInputSnapshot(self) -> None:
180
+ """Request the camera to save a new image file from the input camera stream with overlays. Images
181
+ take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
182
+ space and eventually cause the system to stop working. Clear out images in
183
+ /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
184
+ """
185
+
158
186
  self._inputSaveImgEntry.set(self._inputSaveImgEntry.get() + 1)
159
187
 
160
188
  def takeOutputSnapshot(self) -> None:
189
+ """Request the camera to save a new image file from the output stream with overlays. Images take
190
+ up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
191
+ and eventually cause the system to stop working. Clear out images in
192
+ /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
193
+ """
161
194
  self._outputSaveImgEntry.set(self._outputSaveImgEntry.get() + 1)
162
195
 
163
196
  def getPipelineIndex(self) -> int:
197
+ """Returns the active pipeline index.
198
+
199
+ :returns: The active pipeline index.
200
+ """
201
+
164
202
  return self._pipelineIndexState.get(0)
165
203
 
166
204
  def setPipelineIndex(self, index: int) -> None:
205
+ """Allows the user to select the active pipeline index.
206
+
207
+ :param index: The active pipeline index.
208
+ """
167
209
  self._pipelineIndexRequest.set(index)
168
210
 
169
211
  def getLEDMode(self) -> VisionLEDMode:
212
+ """Returns the current LED mode.
213
+
214
+ :returns: The current LED mode.
215
+ """
216
+
170
217
  mode = self._ledModeState.get()
171
218
  return VisionLEDMode(mode)
172
219
 
173
220
  def setLEDMode(self, led: VisionLEDMode) -> None:
221
+ """Sets the LED mode.
222
+
223
+ :param led: The mode to set to.
224
+ """
225
+
174
226
  self._ledModeRequest.set(led.value)
175
227
 
176
228
  def getName(self) -> str:
229
+ """Returns the name of the camera. This will return the same value that was given to the
230
+ constructor as cameraName.
231
+
232
+ :returns: The name of the camera.
233
+ """
177
234
  return self._name
178
235
 
179
236
  def isConnected(self) -> bool:
237
+ """Returns whether the camera is connected and actively returning new data. Connection status is
238
+ debounced.
239
+
240
+ :returns: True if the camera is actively sending frame data, false otherwise.
241
+ """
242
+
180
243
  curHeartbeat = self._heartbeatEntry.get()
181
244
  now = Timer.getFPGATimestamp()
182
245
 
@@ -197,6 +260,8 @@ class PhotonCamera:
197
260
 
198
261
  _lastVersionTimeCheck = Timer.getFPGATimestamp()
199
262
 
263
+ # Heartbeat entry is assumed to always be present. If it's not present, we
264
+ # assume that a camera with that name was never connected in the first place.
200
265
  if not self._heartbeatEntry.exists():
201
266
  cameraNames = (
202
267
  self._cameraTable.getInstance().getTable(self._tableName).getSubTables()
@@ -222,6 +287,7 @@ class PhotonCamera:
222
287
  True,
223
288
  )
224
289
 
290
+ # Check for connection status. Warn if disconnected.
225
291
  elif not self.isConnected():
226
292
  wpilib.reportWarning(
227
293
  f"PhotonVision coprocessor at path {self._path} is not sending new data.",
@@ -229,45 +295,45 @@ class PhotonCamera:
229
295
  )
230
296
 
231
297
  versionString = self.versionEntry.get(defaultValue="")
232
- localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
233
298
 
234
- remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
299
+ # Check mdef UUID
300
+ localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
301
+ remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
235
302
 
236
- if not remoteUUID:
303
+ if remoteUUID is None:
237
304
  wpilib.reportWarning(
238
305
  f"PhotonVision coprocessor at path {self._path} has not reported a message interface UUID - is your coprocessor's camera started?",
239
306
  True,
240
307
  )
241
-
242
- assert isinstance(remoteUUID, str)
243
- # ntcore hands us a JSON string with leading/trailing quotes - remove those
244
- remoteUUID = remoteUUID.replace('"', "")
245
-
246
- if localUUID != remoteUUID:
247
- # Verified version mismatch
248
-
249
- bfw = """
250
- \n\n\n
251
- >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
252
- >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
253
- >>>
254
- >>> You are running an incompatible version
255
- >>> of PhotonVision on your coprocessor!
256
- >>>
257
- >>> This is neither tested nor supported.
258
- >>> You MUST update PhotonVision,
259
- >>> PhotonLib, or both.
260
- >>>
261
- >>> Your code will now crash.
262
- >>> We hope your day gets better.
263
- >>>
264
- >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
265
- >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
266
- \n\n
267
- """
268
-
269
- wpilib.reportWarning(bfw)
270
-
271
- 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}."
272
- wpilib.reportError(errText, True)
273
- raise Exception(errText)
308
+ else:
309
+ # ntcore hands us a JSON string with leading/trailing quotes - remove those
310
+ remoteUUID = str(remoteUUID).replace('"', "")
311
+
312
+ if localUUID != remoteUUID:
313
+ # Verified version mismatch
314
+
315
+ bfw = """
316
+ \n\n\n
317
+ >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
318
+ >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
319
+ >>>
320
+ >>> You are running an incompatible version
321
+ >>> of PhotonVision on your coprocessor!
322
+ >>>
323
+ >>> This is neither tested nor supported.
324
+ >>> You MUST update PhotonVision,
325
+ >>> PhotonLib, or both.
326
+ >>>
327
+ >>> Your code will now crash.
328
+ >>> We hope your day gets better.
329
+ >>>
330
+ >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
331
+ >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
332
+ \n\n
333
+ """
334
+
335
+ wpilib.reportWarning(bfw)
336
+
337
+ 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}."
338
+ wpilib.reportError(errText, True)
339
+ raise Exception(errText)