From 233a6e4a625a8ef145904522e4663177bcd3d65d Mon Sep 17 00:00:00 2001 From: Shane Smiskol 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 --- 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 660ec3e96..160808cff 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 = []