commit 37f6fe820c655709df6de47c52792bed1ea9a318
author Milind Upadhyay <milind.upadhyay@gmail.com> Thu Apr 14 23:00:44 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Apr 16 19:12:01 2022 -0700
tree 7aefb9501a817fc8a4f23e4f3c3e142bcfd79c7e
parent f3413b74fa846cc27b346791e44f64e900395b2e

Add practice robot extrinsics

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I81f5c3913c9ebfe1a0bc475a45af33b06f532996

y2022/vision/camera_definition.py

diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 8d1c614..11f3918 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -90,7 +90,7 @@
     return base_cam_ext, turret_cam_ext
 
 
-def compute_extrinsic_by_pi(pi_number):
+def compute_extrinsic_by_pi(pi_number, team_number):
     # Defaults for all cameras
     camera_pitch = -35.0 * np.pi / 180.0
     camera_yaw = 0.0
@@ -98,18 +98,31 @@
     # Default camera location to robot origin
     T = np.array([0.0, 0.0, 0.0])
 
-    if pi_number == "pi1":
-        camera_yaw = 90.0 * np.pi / 180.0
-        T = np.array([-11.0 * 0.0254, 5.5 * 0.0254, 29.5 * 0.0254])
-    elif pi_number == "pi2":
-        camera_yaw = 0.0
-        T = np.array([-9.5 * 0.0254, -3.5 * 0.0254, 34.5 * 0.0254])
-    elif pi_number == "pi3":
-        camera_yaw = 179.0 * np.pi / 180.0
-        T = np.array([-9.5 * 0.0254, 3.5 * 0.0254, 34.5 * 0.0254])
-    elif pi_number == "pi4":
-        camera_yaw = -90.0 * np.pi / 180.0
-        T = np.array([-10.25 * 0.0254, -5.0 * 0.0254, 27.5 * 0.0254])
+    if team_number == 971:
+        if pi_number == "pi1":
+            camera_yaw = 90.0 * np.pi / 180.0
+            T = np.array([-11.0 * 0.0254, 5.5 * 0.0254, 29.5 * 0.0254])
+        elif pi_number == "pi2":
+            camera_yaw = 0.0
+            T = np.array([-9.5 * 0.0254, -3.5 * 0.0254, 34.5 * 0.0254])
+        elif pi_number == "pi3":
+            camera_yaw = 179.0 * np.pi / 180.0
+            T = np.array([-9.5 * 0.0254, 3.5 * 0.0254, 34.5 * 0.0254])
+        elif pi_number == "pi4":
+            camera_yaw = -90.0 * np.pi / 180.0
+            T = np.array([-10.25 * 0.0254, -5.0 * 0.0254, 27.5 * 0.0254])
+    elif team_number == 9971:
+        if pi_number == "pi1":
+            camera_yaw = 180.0 * np.pi / 180.0
+            T = np.array([0.0 * 0.0254, 8.5 * 0.0254, 34.0 * 0.0254])
+        elif pi_number == "pi2":
+            camera_yaw = 0.0
+            T = np.array([-9.0 * 0.0254, -3.5 * 0.0254, 35.5 * 0.0254])
+        elif pi_number == "pi3":
+            camera_yaw = 90.0 * np.pi / 180.0
+            T = np.array([-8.0 * 0.0254, 3.0 * 0.0254, 32.0 * 0.0254])
+    else:
+        glog.fatal("Unknown team number for extrinsics")
 
     return compute_extrinsic(camera_pitch, camera_yaw, T, is_turret)
 
@@ -155,7 +168,7 @@
             camera_params = CameraParameters()
             # TODO: Need to add reading in extrinsic camera parameters from json
             camera_params.camera_ext, camera_params.turret_ext = compute_extrinsic_by_pi(
-                node_name)
+                node_name, team_number)
 
             camera_params.node_name = node_name
             camera_params.team_number = team_number