File tree Expand file tree Collapse file tree 1 file changed +4
-2
lines changed Expand file tree Collapse file tree 1 file changed +4
-2
lines changed Original file line number Diff line number Diff line change @@ -211,12 +211,14 @@ def visible_tags(self) -> list[VisibleTag]:
211211
212212 robot_pose = self .chassis .get_pose ()
213213 turret_pose = robot_pose .transformBy (self .robot_to_turret_2d )
214+ turret_translation = turret_pose .translation ()
215+ turret_rotation = turret_pose .rotation ()
214216
215217 for tag in APRILTAGS_2D :
216218 tag_pose = tag .pose
217- turret_to_tag = tag_pose .translation () - turret_pose . translation ()
219+ turret_to_tag = tag_pose .translation () - turret_translation
218220 turret_angle_to_tag = turret_to_tag .angle ()
219- relative_bearing = turret_angle_to_tag - turret_pose . rotation ()
221+ relative_bearing = turret_angle_to_tag - turret_rotation
220222 distance = turret_to_tag .norm ()
221223 relative_facing = tag_pose .rotation () - turret_angle_to_tag
222224 relative_bearing_rad = relative_bearing .radians ()
You can’t perform that action at this time.
0 commit comments