photonlibpy 2025.0.0b3__py3-none-any.whl → 2025.0.0b4__py3-none-any.whl

Sign up to get free protection for your applications and to get access to all the features.
@@ -15,7 +15,22 @@ from .visionTargetSim import VisionTargetSim
15
15
 
16
16
 
17
17
  class VisionSystemSim:
18
+ """A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
19
+ running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
20
+ this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
21
+ should be updated periodically with the robot's current pose in order to publish the simulated
22
+ camera target info.
23
+ """
24
+
18
25
  def __init__(self, visionSystemName: str):
26
+ """A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
27
+ running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
28
+ this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
29
+ should be updated periodically with the robot's current pose in order to publish the simulated
30
+ camera target info.
31
+
32
+ :param visionSystemName: The specific identifier for this vision system in NetworkTables.
33
+ """
19
34
  self.dbgField: Field2d = Field2d()
20
35
  self.bufferLength: seconds = 1.5
21
36
 
@@ -32,12 +47,21 @@ class VisionSystemSim:
32
47
  wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField)
33
48
 
34
49
  def getCameraSim(self, name: str) -> PhotonCameraSim | None:
50
+ """Get one of the simulated cameras."""
35
51
  return self.camSimMap.get(name, None)
36
52
 
37
53
  def getCameraSims(self) -> list[PhotonCameraSim]:
54
+ """Get all the simulated cameras."""
38
55
  return [*self.camSimMap.values()]
39
56
 
40
57
  def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> None:
58
+ """Adds a simulated camera to this vision system with a specified robot-to-camera transformation.
59
+ The vision targets registered with this vision system simulation will be observed by the
60
+ simulated :class:`.PhotonCamera`.
61
+
62
+ :param cameraSim: The camera simulation
63
+ :param robotToCamera: The transform from the robot pose to the camera pose
64
+ """
41
65
  name = cameraSim.getCamera().getName()
42
66
  if name not in self.camSimMap:
43
67
  self.camSimMap[name] = cameraSim
@@ -49,10 +73,15 @@ class VisionSystemSim:
49
73
  )
50
74
 
51
75
  def clearCameras(self) -> None:
76
+ """Remove all simulated cameras from this vision system."""
52
77
  self.camSimMap.clear()
53
78
  self.camTrfMap.clear()
54
79
 
55
80
  def removeCamera(self, cameraSim: PhotonCameraSim) -> bool:
81
+ """Remove a simulated camera from this vision system.
82
+
83
+ :returns: If the camera was present and removed
84
+ """
56
85
  name = cameraSim.getCamera().getName()
57
86
  if name in self.camSimMap:
58
87
  del self.camSimMap[name]
@@ -65,6 +94,14 @@ class VisionSystemSim:
65
94
  cameraSim: PhotonCameraSim,
66
95
  time: seconds = wpilib.Timer.getFPGATimestamp(),
67
96
  ) -> Transform3d | None:
97
+ """Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
98
+ empty optional is returned.
99
+
100
+ :param cameraSim: The specific camera to get the robot-to-camera transform of
101
+ :param timeSeconds: Timestamp in seconds of when the transform should be observed
102
+
103
+ :returns: The transform of this camera, or an empty optional if it is invalid
104
+ """
68
105
  if cameraSim in self.camTrfMap:
69
106
  trfBuffer = self.camTrfMap[cameraSim]
70
107
  sample = trfBuffer.sample(time)
@@ -80,6 +117,13 @@ class VisionSystemSim:
80
117
  cameraSim: PhotonCameraSim,
81
118
  time: seconds = wpilib.Timer.getFPGATimestamp(),
82
119
  ) -> Pose3d | None:
120
+ """Get a simulated camera's position on the field. If the requested camera is invalid, an empty
121
+ optional is returned.
122
+
123
+ :param cameraSim: The specific camera to get the field pose of
124
+
125
+ :returns: The pose of this camera, or an empty optional if it is invalid
126
+ """
83
127
  robotToCamera = self.getRobotToCamera(cameraSim, time)
84
128
  if robotToCamera is None:
85
129
  return None
@@ -93,6 +137,14 @@ class VisionSystemSim:
93
137
  def adjustCamera(
94
138
  self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
95
139
  ) -> bool:
140
+ """Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or
141
+ turret or some other mobile platform.
142
+
143
+ :param cameraSim: The simulated camera to change the relative position of
144
+ :param robotToCamera: New transform from the robot to the camera
145
+
146
+ :returns: If the cameraSim was valid and transform was adjusted
147
+ """
96
148
  if cameraSim in self.camTrfMap:
97
149
  self.camTrfMap[cameraSim].addSample(
98
150
  wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
@@ -102,6 +154,7 @@ class VisionSystemSim:
102
154
  return False
103
155
 
104
156
  def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
157
+ """Reset the transform history for this camera to just the current transform."""
105
158
  now = wpilib.Timer.getFPGATimestamp()
106
159
 
107
160
  def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
@@ -133,12 +186,30 @@ class VisionSystemSim:
133
186
  def addVisionTargets(
134
187
  self, targets: list[VisionTargetSim], targetType: str = "targets"
135
188
  ) -> None:
189
+ """Adds targets on the field which your vision system is designed to detect. The {@link
190
+ PhotonCamera}s simulated from this system will report the location of the camera relative to
191
+ the subset of these targets which are visible from the given camera position.
192
+
193
+ :param targets: Targets to add to the simulated field
194
+ :param type: Type of target (e.g. "cargo").
195
+ """
196
+
136
197
  if targetType not in self.targetSets:
137
198
  self.targetSets[targetType] = targets
138
199
  else:
139
200
  self.targetSets[targetType] += targets
140
201
 
141
202
  def addAprilTags(self, layout: AprilTagFieldLayout) -> None:
203
+ """Adds targets on the field which your vision system is designed to detect. The {@link
204
+ PhotonCamera}s simulated from this system will report the location of the camera relative to
205
+ the subset of these targets which are visible from the given camera position.
206
+
207
+ The AprilTags from this layout will be added as vision targets under the type "apriltag".
208
+ The poses added preserve the tag layout's current alliance origin. If the tag layout's alliance
209
+ origin is changed, these added tags will have to be cleared and re-added.
210
+
211
+ :param tagLayout: The field tag layout to get Apriltag poses and IDs from
212
+ """
142
213
  targets: list[VisionTargetSim] = []
143
214
  for tag in layout.getTags():
144
215
  tag_pose = layout.getTagPose(tag.ID)
@@ -172,9 +243,15 @@ class VisionSystemSim:
172
243
  def getRobotPose(
173
244
  self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
174
245
  ) -> Pose3d | None:
246
+ """Get the robot pose in meters saved by the vision system at this timestamp.
247
+
248
+ :param timestamp: Timestamp of the desired robot pose
249
+ """
250
+
175
251
  return self.robotPoseBuffer.sample(timestamp)
176
252
 
177
253
  def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
254
+ """Clears all previous robot poses and sets robotPose at current time."""
178
255
  if type(robotPose) is Pose2d:
179
256
  robotPose = Pose3d(robotPose)
180
257
  assert type(robotPose) is Pose3d
@@ -186,16 +263,23 @@ class VisionSystemSim:
186
263
  return self.dbgField
187
264
 
188
265
  def update(self, robotPose: Pose2d | Pose3d) -> None:
266
+ """Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
267
+ determine if a new frame should be submitted.
268
+
269
+ :param robotPoseMeters: The simulated robot pose in meters
270
+ """
189
271
  if type(robotPose) is Pose2d:
190
272
  robotPose = Pose3d(robotPose)
191
273
  assert type(robotPose) is Pose3d
192
274
 
275
+ # update vision targets on field
193
276
  for targetType, targets in self.targetSets.items():
194
277
  posesToAdd: list[Pose2d] = []
195
278
  for target in targets:
196
279
  posesToAdd.append(target.getPose().toPose2d())
197
280
  self.dbgField.getObject(targetType).setPoses(posesToAdd)
198
281
 
282
+ # save "real" robot poses over time
199
283
  now = wpilib.Timer.getFPGATimestamp()
200
284
  self.robotPoseBuffer.addSample(now, robotPose)
201
285
  self.dbgField.setRobotPose(robotPose.toPose2d())
@@ -208,17 +292,22 @@ class VisionSystemSim:
208
292
  visTgtPoses2d: list[Pose2d] = []
209
293
  cameraPoses2d: list[Pose2d] = []
210
294
  processed = False
295
+ # process each camera
211
296
  for camSim in self.camSimMap.values():
297
+ # check if this camera is ready to process and get latency
212
298
  optTimestamp = camSim.consumeNextEntryTime()
213
299
  if optTimestamp is None:
214
300
  continue
215
301
  else:
216
302
  processed = True
217
303
 
304
+ # when this result "was" read by NT
218
305
  timestampNt = optTimestamp
219
306
  latency = camSim.prop.estLatency()
307
+ # the image capture timestamp in seconds of this result
220
308
  timestampCapture = timestampNt * 1.0e-6 - latency
221
309
 
310
+ # use camera pose from the image capture timestamp
222
311
  lateRobotPose = self.getRobotPose(timestampCapture)
223
312
  robotToCamera = self.getRobotToCamera(camSim, timestampCapture)
224
313
  if lateRobotPose is None or robotToCamera is None:
@@ -226,8 +315,11 @@ class VisionSystemSim:
226
315
  lateCameraPose = lateRobotPose + robotToCamera
227
316
  cameraPoses2d.append(lateCameraPose.toPose2d())
228
317
 
318
+ # process a PhotonPipelineResult with visible targets
229
319
  camResult = camSim.process(latency, lateCameraPose, allTargets)
320
+ # publish this info to NT at estimated timestamp of receive
230
321
  camSim.submitProcessedFrame(camResult, timestampNt)
322
+ # display debug results
231
323
  for tgt in camResult.getTargets():
232
324
  trf = tgt.getBestCameraToTarget()
233
325
  if trf == Transform3d():
@@ -6,7 +6,16 @@ from ..estimation.targetModel import TargetModel
6
6
 
7
7
 
8
8
  class VisionTargetSim:
9
+ """Describes a vision target located somewhere on the field that your vision system can detect."""
10
+
9
11
  def __init__(self, pose: Pose3d, model: TargetModel, id: int = -1):
12
+ """Describes a fiducial tag located somewhere on the field that your vision system can detect.
13
+
14
+ :param pose: Pose3d of the tag in field-relative coordinates
15
+ :param model: TargetModel which describes the shape of the target(tag)
16
+ :param id: The ID of this fiducial tag
17
+ """
18
+
10
19
  self.pose: Pose3d = pose
11
20
  self.model: TargetModel = model
12
21
  self.fiducialId: int = id
@@ -47,4 +56,5 @@ class VisionTargetSim:
47
56
  return self.model
48
57
 
49
58
  def getFieldVertices(self) -> list[Translation3d]:
59
+ """This target's vertices offset from its field pose."""
50
60
  return self.model.getFieldVertices(self.pose)
photonlibpy/version.py CHANGED
@@ -1,2 +1,2 @@
1
- PHOTONLIB_VERSION="v2025.0.0.beta.3"
2
- PHOTONVISION_VERSION="v2025.0.0-beta-3"
1
+ PHOTONLIB_VERSION="v2025.0.0.beta.4"
2
+ PHOTONVISION_VERSION="v2025.0.0-beta-4"
@@ -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.0b4
4
+ Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.0.0-beta-4 .
5
5
  Home-page: https://photonvision.org
6
6
  Author: Photonvision Development Team
7
7
  Description-Content-Type: text/markdown
@@ -12,6 +12,5 @@ Requires-Dist: robotpy-apriltag<2026,>=2025.0.0b1
12
12
  Requires-Dist: robotpy-cscore<2026,>=2025.0.0b1
13
13
  Requires-Dist: pyntcore<2026,>=2025.0.0b1
14
14
  Requires-Dist: opencv-python; platform_machine != "roborio"
15
- Requires-Dist: robotpy-opencv; platform_machine == "roborio"
16
15
 
17
16
  A Pure-python implementation of PhotonLib
@@ -1,15 +1,15 @@
1
1
  photonlibpy/__init__.py,sha256=WW1OGrrcNXwwxaHSZlkxmhH2GYiQIHHxSxGVTJZhZbY,1136
2
2
  photonlibpy/estimatedRobotPose.py,sha256=X7wF9xdPXGKSVy0MY0qrWZJOEbuZPd721lYp0KXKlP0,1603
3
3
  photonlibpy/packet.py,sha256=5YomViVFwOljL2FGOetWM9FbPc_yCQ15ylzkYlgLIs8,9724
4
- photonlibpy/photonCamera.py,sha256=061meWm5G36_9vByAUBFCor0fR4G24baw306xt_cWB8,10181
4
+ photonlibpy/photonCamera.py,sha256=80-UKakvcgJ79vw80e2yUqZZfoQtpC1_lopnXpfgvOY,12823
5
5
  photonlibpy/photonPoseEstimator.py,sha256=2iMqxPFsQHTsq95yv-WCSv1a6wXNcHPqOyMc4Bu6IG0,12584
6
- photonlibpy/version.py,sha256=A48GfU-qwEM5vulc7mQd7EH8j_mm-7dL2tuMzcswSqg,77
6
+ photonlibpy/version.py,sha256=Ew1xD5CpZetX_p3ReR01MPCVsroMfk2ZpgH42q0Fubg,77
7
7
  photonlibpy/estimation/__init__.py,sha256=pZ-d6fN1DJvT-lRl4FfIos5HAvlzmetIOrGIinrdv7k,223
8
8
  photonlibpy/estimation/cameraTargetRelation.py,sha256=i7DPBXtkZve4ToXQscEIe-5F1oGQ1Qmf5QBaE__EeMQ,1158
9
- photonlibpy/estimation/openCVHelp.py,sha256=6ynO0WbccVxbUCeDu4QgISgipqiqs-erLd14rnSSH-c,6726
10
- photonlibpy/estimation/rotTrlTransform3d.py,sha256=_5q3oMdbYRVBhzxQt5UwyRE7AXkhT3Fth5Ljrj3gANY,1728
11
- photonlibpy/estimation/targetModel.py,sha256=ZfuiB0ZDNM8gMum5nuq6KWyEeFts49jYgMo7-oojjY0,3726
12
- photonlibpy/estimation/visionEstimation.py,sha256=v4WkVtEQd5Q9OWxrcJAseIjP-trMHj_MDzMj4fcfjtw,3019
9
+ photonlibpy/estimation/openCVHelp.py,sha256=h36tOunGaDfs1yp1kAyK-BX0DqdNXZSN8T8ms-ReQas,12277
10
+ photonlibpy/estimation/rotTrlTransform3d.py,sha256=oRYk4V1XF7guNxefJsCPcBWPIkKZl0HYD_Gs86wwToE,2671
11
+ photonlibpy/estimation/targetModel.py,sha256=fIhkf0-_Sfv-KhYEBnhFWMF4Xu84FfF6B-KUEsuuGjQ,6459
12
+ photonlibpy/estimation/visionEstimation.py,sha256=Q4KWdLCV1H0wJUJCzG7OYNhVmk2jR1iPUmX_PFylLdA,4174
13
13
  photonlibpy/generated/MultiTargetPNPResultSerde.py,sha256=CzsCosHUnzxAoSC_sSAqpsXsDp54KW-BClnIXdzfMPc,2506
14
14
  photonlibpy/generated/PhotonPipelineMetadataSerde.py,sha256=9pq5XUEDEoRowCd7sRnuGV7xtugu5HZCRrnwqZG2xXc,2887
15
15
  photonlibpy/generated/PhotonPipelineResultSerde.py,sha256=jTf1obU2jPNDsxvt-tl9Ty_qzJws9Wqron3C1QiHkP8,3137
@@ -17,20 +17,20 @@ photonlibpy/generated/PhotonTrackedTargetSerde.py,sha256=-6vKir_ABDVBGbg8ktM48IK
17
17
  photonlibpy/generated/PnpResultSerde.py,sha256=YoTKdQ51oSdxC-7Poy6hunL0-zkMKvP5uedqaHWPudY,2693
18
18
  photonlibpy/generated/TargetCornerSerde.py,sha256=kziD_rQIwyhzPfgOaDgn-3d87tvtXiAYbBjzu76biYU,2190
19
19
  photonlibpy/generated/__init__.py,sha256=mElM8M88---wxTWO-SRqIJ4EfxN0fdIUwZBZ-UIGuRw,428
20
- photonlibpy/networktables/NTTopicSet.py,sha256=NduTI3c3fG4MuVcY2cuLIoV1i1PSxxYzqBOt24ngS9k,2708
20
+ photonlibpy/networktables/NTTopicSet.py,sha256=TXJyVg6S8gONeuRqX_NI0FdxYw51bNLlFIAQ4Y2wyVg,3104
21
21
  photonlibpy/networktables/__init__.py,sha256=o_LxTdyIylAszMy_zhUtTkXHyu6jqxccccj78d44OrI,35
22
22
  photonlibpy/simulation/__init__.py,sha256=HKJV02of5d8bOnuI7syLzSYtOYge7XUrHSaLvawh99M,227
23
- photonlibpy/simulation/photonCameraSim.py,sha256=BJ870459FdaO1HLkAMfA23W2S8S9axn3rZUPyxQfH40,14078
24
- photonlibpy/simulation/simCameraProperties.py,sha256=gcRfMOH5zQhv8mBElSo8GT7yOC9dA0EsQoJz-1WsCCw,19960
23
+ photonlibpy/simulation/photonCameraSim.py,sha256=5GTPdU7SRAahhtXYHcbm0GJEzARWA7lyBG3v279pqGk,19891
24
+ photonlibpy/simulation/simCameraProperties.py,sha256=bJ6Xvos2glO0-R_a0Wv4eKESnQD13GSYqhwPQdVr1eQ,27082
25
25
  photonlibpy/simulation/videoSimUtil.py,sha256=xMuTvJ2Jx9IoQqmAJi_zUm06MdEwhVpIz9OyzYQp0k4,29
26
- photonlibpy/simulation/visionSystemSim.py,sha256=WZKYdu89IVchcCsXR04AXVQv78F_KgZIQRZ00ocusWE,8814
27
- photonlibpy/simulation/visionTargetSim.py,sha256=AN7jXW3guQfNUu2EiQl23Jj8Mkgn4gYmRphGXL0-Dqk,1734
26
+ photonlibpy/simulation/visionSystemSim.py,sha256=ln1TYVMXUreg6nxrVZJ7lw8puvgLbBBde_ciM2CH5G4,13788
27
+ photonlibpy/simulation/visionTargetSim.py,sha256=FH85fKE4NntowUvssfgZ1KlE-I_3Z-QuAgb2bFqvfdY,2219
28
28
  photonlibpy/targeting/TargetCorner.py,sha256=ouKj3E5uD76OZSNHHuSDzKOY65a8HqtcOsuejH-MVsU,276
29
29
  photonlibpy/targeting/__init__.py,sha256=YzINSpq6A0cjr-yAQcFqHoiYdLGKPFXThlVYlMjY11w,295
30
30
  photonlibpy/targeting/multiTargetPNPResult.py,sha256=Y9rweHtMzoCZ6mv6F8CutQi2Thq5pHN0ydBWvTCsOwY,806
31
31
  photonlibpy/targeting/photonPipelineResult.py,sha256=MbaSyHZTJpoKTtLOZztpSGSt9xWWFqhzgwj8medObVA,2732
32
32
  photonlibpy/targeting/photonTrackedTarget.py,sha256=zCoFp32hX-3GmBYEmsYBQieBoMzXtP2F_55_q0zPOXA,1956
33
- photonlibpy-2025.0.0b3.dist-info/METADATA,sha256=C6jySUa1aCRwXFf3ZKZS8l8C_MV4pnnfBS6Ueho98WM,750
34
- photonlibpy-2025.0.0b3.dist-info/WHEEL,sha256=bFJAMchF8aTQGUgMZzHJyDDMPTO3ToJ7x23SLJa1SVo,92
35
- photonlibpy-2025.0.0b3.dist-info/top_level.txt,sha256=T8Xc6U6he2VjKUAca6zawSkHdUZuLanxYIc4nxw2ctc,12
36
- photonlibpy-2025.0.0b3.dist-info/RECORD,,
33
+ photonlibpy-2025.0.0b4.dist-info/METADATA,sha256=zybpLt0J7EMylub2yKyAugXJ-lt7-uwrAhFV7KGv51U,689
34
+ photonlibpy-2025.0.0b4.dist-info/WHEEL,sha256=bFJAMchF8aTQGUgMZzHJyDDMPTO3ToJ7x23SLJa1SVo,92
35
+ photonlibpy-2025.0.0b4.dist-info/top_level.txt,sha256=T8Xc6U6he2VjKUAca6zawSkHdUZuLanxYIc4nxw2ctc,12
36
+ photonlibpy-2025.0.0b4.dist-info/RECORD,,