photonlibpy 2025.0.0b3__py3-none-any.whl → 2025.0.0b5__py3-none-any.whl
Sign up to get free protection for your applications and to get access to all the features.
- photonlibpy/estimation/openCVHelp.py +110 -1
- photonlibpy/estimation/rotTrlTransform3d.py +19 -0
- photonlibpy/estimation/targetModel.py +63 -0
- photonlibpy/estimation/visionEstimation.py +22 -0
- photonlibpy/generated/PhotonPipelineResultSerde.py +1 -3
- photonlibpy/networktables/NTTopicSet.py +7 -0
- photonlibpy/photonCamera.py +102 -36
- photonlibpy/py.typed +1 -0
- photonlibpy/simulation/photonCameraSim.py +109 -3
- photonlibpy/simulation/simCameraProperties.py +142 -1
- photonlibpy/simulation/visionSystemSim.py +92 -0
- photonlibpy/simulation/visionTargetSim.py +10 -0
- photonlibpy/version.py +2 -2
- {photonlibpy-2025.0.0b3.dist-info → photonlibpy-2025.0.0b5.dist-info}/METADATA +2 -3
- {photonlibpy-2025.0.0b3.dist-info → photonlibpy-2025.0.0b5.dist-info}/RECORD +17 -16
- {photonlibpy-2025.0.0b3.dist-info → photonlibpy-2025.0.0b5.dist-info}/WHEEL +0 -0
- {photonlibpy-2025.0.0b3.dist-info → photonlibpy-2025.0.0b5.dist-info}/top_level.txt +0 -0
@@ -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
|
-
|
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()
|
photonlibpy/photonCamera.py
CHANGED
@@ -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
|
-
|
299
|
+
# Check mdef UUID
|
300
|
+
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
|
301
|
+
remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
|
235
302
|
|
236
|
-
if
|
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
|
-
|
243
|
-
|
244
|
-
|
245
|
-
|
246
|
-
|
247
|
-
|
248
|
-
|
249
|
-
|
250
|
-
|
251
|
-
|
252
|
-
|
253
|
-
|
254
|
-
|
255
|
-
|
256
|
-
|
257
|
-
|
258
|
-
|
259
|
-
|
260
|
-
|
261
|
-
|
262
|
-
|
263
|
-
|
264
|
-
|
265
|
-
|
266
|
-
|
267
|
-
|
268
|
-
|
269
|
-
|
270
|
-
|
271
|
-
|
272
|
-
|
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)
|
photonlibpy/py.typed
ADDED
@@ -0,0 +1 @@
|
|
1
|
+
|