calculate correct path offset for TICI (commaai#544)

README.md

@@ -1,4 +1,4 @@
-# Stock Additions - [2021-12-19](/SA_RELEASES.md) (0.8.13)
+# Stock Additions - [2021-12-28](/SA_RELEASES.md) (0.8.13)
 
 Stock Additions is a fork of openpilot designed to be minimal in design while boasting various feature additions and behavior improvements over stock. I have a 2017 Toyota Corolla with comma pedal, so most of my changes are designed to improve the longitudinal performance.
 

SA_RELEASES.md

@@ -1,5 +1,7 @@
 Stock Additions 0.8.13
 ===
+## - 2021-12-28, 3:00am MST Notes
+ * Use correct path offset for comma three, may reduce slight left hugging
 ## - 2021-12-24, 3:23am MST Notes
  * Initialize controls immediately to send required ACC messages to resolve some Toyota cruise faults
 ## - 2021-12-21, 2:46am MST Notes

selfdrive/controls/lib/lane_planner.py

@@ -10,22 +10,14 @@
 
 
 TRAJECTORY_SIZE = 33
-# camera offset is meters from center car to camera
+# model path is 0.06 m left of car center
+MODEL_PATH_OFFSET = 0.06
 if EON:
-  STANDARD_CAMERA_OFFSET = 0.06  # do NOT change this. edit with opEdit
-  STANDARD_PATH_OFFSET = 0.0  # do NOT change this. edit with opEdit
-  # CAMERA_OFFSET = 0.06
-  # PATH_OFFSET = 0.0
+  CAMERA_OFFSET = 0.06
 elif TICI:
-  STANDARD_CAMERA_OFFSET = -0.04  # do NOT change this. edit with opEdit
-  STANDARD_PATH_OFFSET = -0.04  # do NOT change this. edit with opEdit
-  # CAMERA_OFFSET = -0.04
-  # PATH_OFFSET = -0.04
+  CAMERA_OFFSET = -0.04
 else:
-  STANDARD_CAMERA_OFFSET = 0.0  # do NOT change this. edit with opEdit
-  STANDARD_PATH_OFFSET = 0.0  # do NOT change this. edit with opEdit
-  # CAMERA_OFFSET = 0.0
-  # PATH_OFFSET = 0.0
+  CAMERA_OFFSET = 0.0
 
 
 class LanePlanner:
@@ -50,15 +42,13 @@ def __init__(self, wide_camera=False):
     self.l_lane_change_prob = 0.
     self.r_lane_change_prob = 0.
 
-    # self.camera_offset = -STANDARD_CAMERA_OFFSET if wide_camera else STANDARD_CAMERA_OFFSET
-    if TICI:
-      self.path_offset = -STANDARD_PATH_OFFSET if wide_camera else STANDARD_PATH_OFFSET
+    self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET
+    self.path_offset = self.camera_offset - MODEL_PATH_OFFSET
 
   def parse_model(self, md):
     if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
-      self.camera_offset = clip(self.op_params.get('camera_offset'), -0.3, 0.3)  # update camera offset
-      if not TICI:  # TODO: make sure this is correct
-        self.path_offset = self.camera_offset - STANDARD_CAMERA_OFFSET + STANDARD_PATH_OFFSET  # offset path
+      self.camera_offset = clip(self.op_params.get('camera_offset'), -0.5, 0.5)  # update camera offset
+      self.path_offset = self.camera_offset - MODEL_PATH_OFFSET
 
       self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
       # left and right ll x is the same