From 33ff1c9783224860fa9d3548eaad2f47b453465b Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Thu, 22 Aug 2019 18:05:16 +1000 Subject: [PATCH] =?UTF-8?q?=E8=AA=BF=E6=95=B4camera=5Foffset=20=E5=96=AE?= =?UTF-8?q?=E4=BD=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- selfdrive/controls/lib/lane_planner.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 14d1ddf5e..c61f7217f 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,10 +1,11 @@ from common.numpy_fast import interp import numpy as np from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv +from common.realtime import sec_since_boot from common.params import Params params = Params() -CAMERA_OFFSET = int(params.get("DragonCameraOffset")) +CAMERA_OFFSET = 0.06 # m from center car to camera def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): @@ -42,6 +43,9 @@ class LanePlanner(object): self._path_pinv = compute_path_pinv() self.x_points = np.arange(50) + self.ts_last_check = 0. + self.camera_offset = 0.06 + def parse_model(self, md): if len(md.leftLane.poly): self.l_poly = np.array(md.leftLane.poly) @@ -55,9 +59,13 @@ class LanePlanner(object): self.r_prob = md.rightLane.prob # right line prob def update_lane(self, v_ego): + ts = sec_since_boot() + if ts - self.ts_last_check > 3.: + self.camera_offset = int(params.get("DragonCameraOffset")) * 0.01 + self.ts_last_check = ts # only offset left and right lane lines; offsetting p_poly does not make sense - self.l_poly[3] += CAMERA_OFFSET - self.r_poly[3] += CAMERA_OFFSET + self.l_poly[3] += self.camera_offset + self.r_poly[3] += self.camera_offset self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob