Merge pull request #9 from eFiniLan/0813-model

0813 model - Cont.

README.md

@@ -42,8 +42,9 @@ I encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for
 * Services are not optimized for resource usage, and using all services may result in overheating issues.
 * Language files can only be generated in a PC due to missing Qt5 tools.
 * webjoystick is disabled as it requires additional python modules. (aiohttp and others)
-* Starting from August 7th, 2023, comma has removed ESP/GPS support from Pandas. You can find more details about this change in this link. 
+* Starting from August 7th, 2023, comma has removed ESP/GPS support from Pandas. You can find more details about this change in this [link](https://github.com/commaai/panda/commit/c66b98b2a67441faa4cfcd36c3c9d9f90474cd08).
   * Going forward, I will focus solely on maintaining the safety aspects of the code, ensuring that vehicle support and safety declarations remain up to date.
+* For safety concern, End-to-End / vision only longitudinal control only available in 0.8.16 driving model.
 
 
 ## Configuration

selfdrive/controls/lib/legacy_lateral_planner.py

@@ -9,7 +9,6 @@
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
 from cereal import log
-from openpilot.common.params import Params
 
 STEER_RATE_COST = {
   "chrysler": 0.7,
@@ -29,9 +28,6 @@ class LateralPlanner:
   def __init__(self, CP, debug=False):
     self.LP = LanePlanner()
     self.DH = DesireHelper()
-    self._dp_lat_lane_priority_mode = Params().get_bool("dp_lat_lane_priority_mode")
-    self._dp_lat_lane_priority_mode_active = False
-    self._dp_lat_lane_priority_mode_active_prev = False
 
     self.last_cloudlog_t = 0
     try:
@@ -45,8 +41,6 @@ def __init__(self, CP, debug=False):
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
-    # dp // mapd - for vision turn controller
-    self._d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
 
     self.lat_mpc = LateralMpc()
     self.reset_mpc(np.zeros(4))
@@ -78,11 +72,7 @@ def update(self, sm):
       self.LP.lll_prob *= self.DH.lane_change_ll_prob
       self.LP.rll_prob *= self.DH.lane_change_ll_prob
 
-    # dp - check laneline prob when priority is on
     use_laneline = False
-    if self._dp_lat_lane_priority_mode:
-      self._update_laneless_laneline_mode()
-      use_laneline = self._dp_lat_lane_priority_mode_active
 
     # Calculate final driving path and set MPC costs
     if use_laneline:
@@ -95,8 +85,6 @@ def update(self, sm):
       heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
       self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
 
-    self._d_path_w_lines_xyz = d_path_xyz
-
     y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
     heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
@@ -146,30 +134,8 @@ def publish(self, sm, pm):
     lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
 
     lateralPlan.desire = self.DH.desire
-    lateralPlan.useLaneLines = self._dp_lat_lane_priority_mode and self._dp_lat_lane_priority_mode_active
+    lateralPlan.useLaneLines = False
     lateralPlan.laneChangeState = self.DH.lane_change_state
     lateralPlan.laneChangeDirection = self.DH.lane_change_direction
 
     pm.send('lateralPlan', plan_send)
-
-    # dp - extension
-    plan_ext_send = messaging.new_message('lateralPlanExt')
-
-    lateralPlanExt = plan_ext_send.lateralPlanExt
-    # for vision turn controller
-    lateralPlanExt.dPathWLinesX = [float(x) for x in self._d_path_w_lines_xyz[:, 0]]
-    lateralPlanExt.dPathWLinesY = [float(y) for y in self._d_path_w_lines_xyz[:, 1]]
-
-    pm.send('lateralPlanExt', plan_ext_send)
-
-  def _update_laneless_laneline_mode(self):
-    # decide what mode should we use
-    if (self.LP.lll_prob + self.LP.rll_prob)/2 < 0.3:
-      self._dp_lat_lane_priority_mode_active = False
-    if (self.LP.lll_prob + self.LP.rll_prob)/2 > 0.5:
-      self._dp_lat_lane_priority_mode_active = True
-
-    # perform reset mpc
-    if self._dp_lat_lane_priority_mode_active != self._dp_lat_lane_priority_mode_active_prev:
-      self.reset_mpc()
-    self._dp_lat_lane_priority_mode_active_prev = self._dp_lat_lane_priority_mode_active

selfdrive/controls/plannerd.py

@@ -47,6 +47,7 @@ def plannerd_thread(sm=None, pm=None):
 
   longitudinal_planner = LongitudinalPlanner(CP)
   lateral_planner = LateralPlanner(CP, debug=debug_mode)
+  is_old_model = Params().get_bool("dp_0813")
 
   if sm is None:
     sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
@@ -63,7 +64,8 @@ def plannerd_thread(sm=None, pm=None):
       lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
-      publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
+      if not is_old_model:
+        publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
 
 def main(sm=None, pm=None):
   plannerd_thread(sm, pm)