return lat

selfdrive/controls/lib/lane_planner_2.py

@@ -75,14 +75,6 @@ class LanePlanner:
     self.params = Params()
     self.camera_offset = self.params.get_int("CameraOffset") * 0.01
 
-    # 障碍物绕行参数
-    # 绕行要“明显”，需要更快的响应；时间常数过大时偏移会被抹平
-    self.obstacle_avoidance_offset = FirstOrderFilter(0.0, 0.4, DT_MDL)
-    self.obstacle_offset_left = 0.0
-    self.obstacle_offset_right = 0.0
-    self.last_avoidance_time = 0.0  # 记录最后一次绕行时间
-    self.avoidance_cooldown = 2.0  # 绕行结束后的冷却时间（秒）
-
   def parse_model(self, md):
 
     lane_lines = md.laneLines
@@ -111,137 +103,7 @@ class LanePlanner:
       self.l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       self.r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
 
-  def calculate_obstacle_avoidance_offset(self, leads_left, leads_right, v_ego, lead_one=None):
+  def get_d_path(self, CS, v_ego, path_t, path_xyz, curve_speed):
-    """计算障碍物绕行偏移量，包含对向来车的避让优化"""
-
-    def _lead_field(lead, name, default=0.0):
-      # 既兼容 dict，又兼容 capnp/对象属性
-      if isinstance(lead, dict):
-        return lead.get(name, default)
-      return getattr(lead, name, default)
-
-    offset_left = 0.0
-    offset_right = 0.0
-    oncoming_detected = False
-
-    # -----------------------
-    # 1. 处理前方主目标（本车车道内障碍物）
-    # -----------------------
-    if lead_one is not None:
-      try:
-        d_rel = float(_lead_field(lead_one, 'dRel', 100.0))
-        v_lead = float(_lead_field(lead_one, 'vLead', 0.0))
-        d_path = float(_lead_field(lead_one, 'dPath', 10.0))
-
-        if 5.0 < d_rel < 50.0 and abs(d_path) < 1.5:
-          v_lead_kph = v_lead * 3.6
-
-          # 识别障碍物类型（行人/电动车/一般车辆）
-          if v_lead_kph < 6.0:
-            vulnerable_factor = 2.5
-          elif v_lead_kph < 25.0:
-            vulnerable_factor = 2.0
-          elif v_lead_kph < 40.0:
-            vulnerable_factor = 1.5
-          else:
-            vulnerable_factor = 1.0
-
-          distance_factor = np.interp(d_rel, [5.0, 30.0], [1.0, 0.3])
-
-          # 左侧障碍物 → 向右偏；右侧障碍物 → 向左偏
-          if d_path < -0.3:
-            offset_right = max(offset_right, 0.8 * distance_factor * vulnerable_factor)
-          elif d_path > 0.3:
-            offset_left = max(offset_left, 0.8 * distance_factor * vulnerable_factor)
-      except Exception:
-        pass
-
-    # -----------------------
-    # 2. 侧向障碍物 + 对向来车处理
-    #    left 列表代表车辆左侧一带，right 代表车辆右侧一带
-    # -----------------------
-
-    def _process_side_leads(leads, is_left_side):
-      nonlocal offset_left, offset_right, oncoming_detected
-
-      for lead in leads:
-        status = bool(_lead_field(lead, 'status', False))
-        if not status:
-          continue
-
-        d_rel = float(_lead_field(lead, 'dRel', 100.0))
-        v_lead = float(_lead_field(lead, 'vLead', 0.0))
-        v_rel = float(_lead_field(lead, 'vRel', 0.0))
-        d_path = float(abs(_lead_field(lead, 'dPath', 10.0)))
-
-        if d_rel >= 80.0 or d_path >= 4.0:
-          continue
-
-        v_lead_kph = v_lead * 3.6
-
-        # 基础类型权重：行人/电动车/普通车
-        if v_lead_kph < 6.0:
-          vulnerable_factor = 2.5
-          min_safe_distance = 2.0
-        elif v_lead_kph < 25.0:
-          vulnerable_factor = 2.0
-          min_safe_distance = 1.5
-        elif v_lead_kph < 40.0:
-          vulnerable_factor = 1.5
-          min_safe_distance = 1.2
-        else:
-          vulnerable_factor = 1.0
-          min_safe_distance = 1.0
-
-        # 静止/缓慢目标增强
-        if abs(v_rel) < 2.0 and v_lead_kph < 10.0:
-          vulnerable_factor *= 1.4
-
-        # 对向来车判定：相对速度为负且较大（更早触发），并且目标自身速度不低
-        is_oncoming = (v_rel < -3.0 and v_lead_kph > 20.0)
-        if is_oncoming:
-          # 对向车优先级再提升一些，并允许在更远距离就开始偏移
-          oncoming_detected = True
-          vulnerable_factor *= 1.8
-          distance_factor = np.interp(d_rel, [12.0, 90.0], [1.6, 0.35])
-        else:
-          distance_factor = np.interp(d_rel, [3.0, 40.0], [1.3, 0.2])
-
-        lateral_factor = np.interp(d_path, [0.3, 3.8], [1.2, 0.2])
-        if d_path < min_safe_distance:
-          lateral_factor *= 1.8
-
-        # 左侧列表 → 向右偏；右侧列表 → 向左偏
-        base_gain = 1.35 if is_oncoming else 1.0
-        avoidance_strength = base_gain * 0.95 * distance_factor * lateral_factor * vulnerable_factor
-        if is_left_side:
-          offset_right = max(offset_right, avoidance_strength)
-        else:
-          offset_left = max(offset_left, avoidance_strength)
-
-    if leads_left is not None:
-      _process_side_leads(leads_left, is_left_side=True)
-    if leads_right is not None:
-      _process_side_leads(leads_right, is_left_side=False)
-
-    # 计算最终偏移
-    final_offset = offset_left - offset_right
-    
-    # 速度自适应
-    # 对向场景下不要在高速被过度削弱，否则体感“不明显”
-    if oncoming_detected:
-      speed_factor = np.interp(v_ego * 3.6, [5, 30, 100], [1.8, 1.55, 1.0])
-    else:
-      speed_factor = np.interp(v_ego * 3.6, [5, 30, 80], [1.6, 1.3, 0.8])
-    final_offset *= speed_factor
-    
-    # 限制最大偏移量
-    max_offset = np.interp(v_ego * 3.6, [10, 60], [1.35, 0.9])
-    if oncoming_detected:
-      max_offset *= 1.35
-    return np.clip(final_offset, -max_offset, max_offset)
-
-  def get_d_path(self, CS, v_ego, path_t, path_xyz, curve_speed, leads_left=None, leads_right=None, lead_one=None):
     #if v_ego > 0.1:
     #  self.lane_width_updated_count = max(0, self.lane_width_updated_count - 1)
     # Reduce reliance on lanelines that are too far apart or
@@ -370,31 +232,6 @@ class LanePlanner:
     #  self.lane_width_left_filtered.x, self.lane_width, self.lane_width_right_filtered.x)
 
     adjustLaneTime = self.params.get_float("LatMpcInputOffset") * 0.01 # 0.06
-    
-    # 计算障碍物绕行偏移（在车道线处理之前）
-    obstacle_offset = 0.0
-    has_obstacle = False
-    try:
-      if leads_left is not None and leads_right is not None:
-        obstacle_offset = self.calculate_obstacle_avoidance_offset(leads_left, leads_right, v_ego, lead_one)
-        
-        # 检测是否有需要绕行的障碍物
-        if abs(obstacle_offset) > 0.05:
-          has_obstacle = True
-          self.last_avoidance_time = 0.0  # 重置计时器
-        else:
-          self.last_avoidance_time += DT_MDL
-        
-        # 如果障碍物消失，平滑回归原车道
-        if not has_obstacle and self.last_avoidance_time < self.avoidance_cooldown:
-          # 在冷却期内，逐渐减小绕行偏移
-          decay_factor = 1.0 - (self.last_avoidance_time / self.avoidance_cooldown)
-          obstacle_offset = self.obstacle_avoidance_offset.x * decay_factor
-        
-        self.obstacle_avoidance_offset.update(obstacle_offset)
-    except Exception:
-      pass
-    
     laneline_active = False
     self.d_prob_count = self.d_prob_count + 1 if self.d_prob > 0.3 else 0
     if self.lanefull_mode and self.d_prob_count > int(1 / DT_MDL):
@@ -409,13 +246,10 @@ class LanePlanner:
           lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
           path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
 
-    # 应用障碍物绕行偏移（优先级高于车道线）
-    if abs(self.obstacle_avoidance_offset.x) > 0.03:
-      path_xyz[:,1] += self.obstacle_avoidance_offset.x
 
     path_xyz[:, 1] += (self.camera_offset + self.lane_offset_filtered.x)
 
-    self.offset_total = self.lane_offset_filtered.x + self.obstacle_avoidance_offset.x
+    self.offset_total = self.lane_offset_filtered.x
 
     return path_xyz, laneline_active
 

selfdrive/controls/lib/lateral_planner.py

@@ -60,7 +60,6 @@ class LateralPlanner:
 
     self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeed")
     self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
-    self.bypass_lat_offset = 0.0
     self.useLaneLineMode = False
     self.plan_a = np.zeros((TRAJECTORY_SIZE, ))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
@@ -161,29 +160,7 @@ class LateralPlanner:
     self.latDebugText = self.LP.debugText
     #self.lanelines_active = True if self.LP.d_prob > 0.3 and self.LP.lanefull_mode else False
 
-    # Bypass lateral assist (no new model): when a close slow lead exists and
+    self.path_xyz[:, 1] += self.pathOffset
-    # lane-change intent is active, add a small temporary lateral offset to help
-    # the vehicle commit to bypass trajectory earlier.
-    lead = sm['radarState'].leadOne
-    lane_change_active = md.meta.desire != log.Desire.none or carrot.desireState > 0.7
-    lead_slow_close = lead.status and lead.dRel < 45.0 and (self.v_ego - lead.vLead) > 1.0 and self.v_ego < (50.0 / 3.6)
-
-    if lane_change_active and lead_slow_close:
-      # choose offset direction from current model desire state (left/right)
-      if md.meta.desire == log.Desire.laneChangeLeft:
-        target_bypass_offset = 0.28
-      elif md.meta.desire == log.Desire.laneChangeRight:
-        target_bypass_offset = -0.28
-      else:
-        target_bypass_offset = 0.0
-    else:
-      target_bypass_offset = 0.0
-
-    # smooth offset transitions to avoid lateral jerk
-    alpha = np.clip(DT_MDL / 0.5, 0.0, 1.0)
-    self.bypass_lat_offset += alpha * (target_bypass_offset - self.bypass_lat_offset)
-
-    self.path_xyz[:, 1] += (self.pathOffset + self.bypass_lat_offset)
 
     self.lat_mpc.set_weights(self.lateralPathCost, self.lateralMotionCost,
                              LATERAL_ACCEL_COST, LATERAL_JERK_COST,