From 56b3c7f790209b8a306e366defc8b672808f1fe0 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Mon, 10 Jan 2022 14:08:45 -0700
Subject: [PATCH] LanePlanner: offsets cleanup (#23459)

* update comments

* model y axis was flipped so flip the offsets as well

* fix
old-commit-hash: 233a6e4a625a8ef145904522e4663177bcd3d65d
---
 selfdrive/controls/lib/lane_planner.py | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index 660ec3e965..160808cff0 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -9,12 +9,14 @@ from selfdrive.swaglog import cloudlog
 
 TRAJECTORY_SIZE = 33
 # camera offset is meters from center car to camera
+# model path is in the frame of EON's camera. TICI is 0.1 m away,
+# however the average measured path difference is 0.04 m
 if EON:
-  CAMERA_OFFSET = 0.06
+  CAMERA_OFFSET = -0.06
   PATH_OFFSET = 0.0
 elif TICI:
-  CAMERA_OFFSET = -0.04
-  PATH_OFFSET = -0.04
+  CAMERA_OFFSET = 0.04
+  PATH_OFFSET = 0.04
 else:
   CAMERA_OFFSET = 0.0
   PATH_OFFSET = 0.0
@@ -49,9 +51,8 @@ class LanePlanner:
       self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
       # left and right ll x is the same
       self.ll_x = lane_lines[1].x
-      # only offset left and right lane lines; offsetting path does not make sense
-      self.lll_y = np.array(lane_lines[1].y) - self.camera_offset
-      self.rll_y = np.array(lane_lines[2].y) - self.camera_offset
+      self.lll_y = np.array(lane_lines[1].y) + self.camera_offset
+      self.rll_y = np.array(lane_lines[2].y) + self.camera_offset
       self.lll_prob = md.laneLineProbs[1]
       self.rll_prob = md.laneLineProbs[2]
       self.lll_std = md.laneLineStds[1]
@@ -65,7 +66,7 @@ class LanePlanner:
   def get_d_path(self, v_ego, path_t, path_xyz):
     # Reduce reliance on lanelines that are too far apart or
     # will be in a few seconds
-    path_xyz[:, 1] -= self.path_offset
+    path_xyz[:, 1] += self.path_offset
     l_prob, r_prob = self.lll_prob, self.rll_prob
     width_pts = self.rll_y - self.lll_y
     prob_mods = []