spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

LanePlanner: offsets cleanup (#23459) 

* update comments

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

* fix
pull/23490/head
Shane Smiskol 2022-01-10 14:08:45 -07:00 committed by GitHub
parent ed5e58c48a
commit 233a6e4a62
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
selfdrive/controls/lib/lane_planner.py
View File

@ -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 = []