Lateral Planner: Fix Custom Offsets and Path Offset in all modes (#309)

* Lateral Planner: Fix Custom Offsets and Path Offset in all modes

* Update init values

selfdrive/controls/lib/lane_planner.py

@@ -1,9 +1,10 @@
 import numpy as np
 from cereal import log
-from common.filter_simple import FirstOrderFilter
-from common.numpy_fast import interp
-from common.realtime import DT_MDL
-from system.swaglog import cloudlog
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.system.swaglog import cloudlog
 
 
 TRAJECTORY_SIZE = 33
@@ -33,10 +34,29 @@ class LanePlanner:
     self.l_lane_change_prob = 0.
     self.r_lane_change_prob = 0.
 
-    self.camera_offset = -CAMERA_OFFSET
-    self.path_offset = -PATH_OFFSET
+    self.camera_offset = CAMERA_OFFSET
+    self.path_offset = PATH_OFFSET
+
+    self.param_s = Params()
+    self.custom_offsets = self.param_s.get_bool("CustomOffsets")
+    self._frame = 0
+
+  def update_custom_offsets(self):
+    self._frame += 1
+    if self._frame % 50 == 0:
+      self.custom_offsets = self.param_s.get_bool("CustomOffsets")
+      if not self.custom_offsets:
+        self._frame = 0
+        self.camera_offset = CAMERA_OFFSET
+        self.path_offset = PATH_OFFSET
+        return
+
+      self.camera_offset = float(self.param_s.get("CameraOffset", encoding="utf8")) * 0.01
+      self.path_offset = float(self.param_s.get("PathOffset", encoding="utf8")) * 0.01
+      self._frame = 0
 
   def parse_model(self, md):
+    self.update_custom_offsets()
     lane_lines = md.laneLines
     if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
       self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2

selfdrive/controls/lib/lateral_planner.py

@@ -115,12 +115,11 @@ class LateralPlanner:
     low_speed = v_ego_car < 10 * CV.MPH_TO_MS
 
     if not self.get_dynamic_lane_profile(sm['longitudinalPlan']) and not low_speed:
-      d_path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz)
+      self.path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz)
       self.dynamic_lane_profile_status = False
     else:
-      d_path_xyz = self.path_xyz
+      self.path_xyz[:, 1] += self.LP.path_offset
       self.dynamic_lane_profile_status = True
-    self.path_xyz = d_path_xyz
 
     self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
                              LATERAL_ACCEL_COST, LATERAL_JERK_COST,

selfdrive/manager/manager.py

@@ -58,7 +58,7 @@ def manager_init() -> None:
     ("CustomTorqueLateral", "0"),
     ("CameraControl", "2"),
     ("CameraControlToggle", "0"),
-    ("CameraOffset", "0"),
+    ("CameraOffset", "4"),
     ("CarModel", ""),
     ("CarModelText", ""),
     ("ChevronInfo", "1"),

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -846,10 +846,10 @@ void BrightnessControl::refresh() {
 // Camera Offset Value
 CameraOffset::CameraOffset() : SPOptionControl (
   "CameraOffset",
-  tr("Camera Offset (cm)"),
-  tr("Hack to trick vehicle to be left or right biased in its lane. Decreasing the value will make the car keep more left, increasing will make it keep more right. Changes take effect immediately."),
+  tr("Camera Offset - Laneful Only (cm)"),
+  tr("Hack to trick vehicle to be left or right biased in its lane. Decreasing the value will make the car keep more left, increasing will make it keep more right. Changes take effect immediately. Default: +4 cm"),
   "../assets/offroad/icon_blank.png",
-  {-10, 10}) {
+  {-20, 20}) {
 
   refresh();
 }