Precompute 2D pose fiducial flattening

Author: auscompgeek

components/vision.py

@@ -22,7 +22,7 @@
 from components.chassis import ChassisComponent
 from utilities.caching import HasPerLoopCache, cache_per_loop
 from utilities.functions import clamp
-from utilities.game import APRILTAGS, apriltag_layout
+from utilities.game import APRILTAGS_2D, apriltag_layout
 from utilities.rev import configure_through_bore_encoder
 
 
@@ -212,8 +212,8 @@ def visible_tags(self) -> list[VisibleTag]:
         robot_pose = self.chassis.get_pose()
         turret_pose = robot_pose.transformBy(self.robot_to_turret_2d)
 
-        for tag in APRILTAGS:
-            tag_pose = tag.pose.toPose2d()
+        for tag in APRILTAGS_2D:
+            tag_pose = tag.pose
             turret_to_tag = tag_pose.translation() - turret_pose.translation()
             turret_angle_to_tag = turret_to_tag.angle()
             relative_bearing = turret_angle_to_tag - turret_pose.rotation()
@@ -241,7 +241,7 @@ def visible_tags(self) -> list[VisibleTag]:
             ):
                 # Test for relative facing is more than 90 degrees because we don't want to be too
                 # close to parallel to the tag
-                tags_in_view.append(VisibleTag(tag.ID, relative_bearing_rad, distance))
+                tags_in_view.append(VisibleTag(tag.id, relative_bearing_rad, distance))
 
         return tags_in_view
 

utilities/game.py

@@ -1,5 +1,6 @@
 """Descriptions of the field and match state."""
 
+import dataclasses
 import math
 import typing
 
@@ -27,6 +28,18 @@
 
 APRILTAGS = apriltag_layout.getTags()
 
+
+@dataclasses.dataclass(slots=True)
+class Tag2d:
+    id: TagId
+    pose: Pose2d
+
+
+APRILTAGS_2D = [
+    Tag2d(typing.cast(TagId, tag.ID), tag.pose.toPose2d()) for tag in APRILTAGS
+]
+
+
 L3_TAGS = [7, 9, 11, 18, 20, 22]
 L2_TAGS = [6, 8, 10, 17, 19, 21]