Skip to content

Commit 476f897

Browse files
committed
vision: Avoid redundant calls into C++
1 parent 809f734 commit 476f897

File tree

1 file changed

+5
-5
lines changed

1 file changed

+5
-5
lines changed

components/vision.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -213,12 +213,12 @@ def visible_tags(self) -> list[VisibleTag]:
213213
turret_pose = robot_pose.transformBy(self.robot_to_turret_2d)
214214

215215
for tag in APRILTAGS:
216-
turret_to_tag = (
217-
tag.pose.toPose2d().translation() - turret_pose.translation()
218-
)
219-
relative_bearing = turret_to_tag.angle() - turret_pose.rotation()
216+
tag_pose = tag.pose.toPose2d()
217+
turret_to_tag = tag_pose.translation() - turret_pose.translation()
218+
turret_angle_to_tag = turret_to_tag.angle()
219+
relative_bearing = turret_angle_to_tag - turret_pose.rotation()
220220
distance = turret_to_tag.norm()
221-
relative_facing = tag.pose.toPose2d().rotation() - turret_to_tag.angle()
221+
relative_facing = tag_pose.rotation() - turret_angle_to_tag
222222
relative_bearing_rad = relative_bearing.radians()
223223
# Make the angle less than the max rotation, then see if we are above the min too
224224
in_rotation_range = False

0 commit comments

Comments
 (0)