File tree Expand file tree Collapse file tree 1 file changed +5
-5
lines changed Expand file tree Collapse file tree 1 file changed +5
-5
lines changed Original file line number Diff line number Diff 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
You can’t perform that action at this time.
0 commit comments