simpler logic?

[eFiniLan]

https://github.com/sunnyhaibin/openpilot/blob/f106563174bba424f3afa0c83006fbf11034eba4/selfdrive/controls/lib/lateral_planner.py#L184

Hey Sunny,

I've simplified your DLP code a bit (too many repeated codes), could you please take a look and let me know if anything is missing?

    # Turn off lanes during lane change
    if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
      self.LP.lll_prob *= self.lane_change_ll_prob
      self.LP.rll_prob *= self.lane_change_ll_prob
    if self.get_dlp_laneless_mode(): # this is where we call the method below
     # laneline logic 
     d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
      self.laneless_mode_is_e2e = True # store auto mode value
    else:
      # laneless logic
      d_path_xyz = self.path_xyz
      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
      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.laneless_mode_is_e2e = False # store auto mode value

  # a method to decide which mode (e2e or lane) to go
  def get_dlp_laneless_mode(self):
    if self.laneless_mode == 1: # e2e
      return True
    if self.laneless_mode == 0: # lane
      return False
    elif self.laneless_mode == 2: # auto
      # only while lane change is off
      if self.lane_change_state == LaneChangeState.off:
        # lane probability too low, we switch to laneless mode
        if (self.LP.lll_prob + self.LP.rll_prob)/2 < 0.3:
          self.laneless_mode_buffer = True
        if (self.LP.lll_prob + self.LP.rll_prob)/2 > 0.5:
          self.laneless_mode_buffer = False
        if self.laneless_mode_buffer: # in buffer mode, always laneless
          return True
    return False

thanks for the awesome work! :)

[sunnyhaibin]

Hi eFini,

Thanks for looking into this! Just tested this today and it worked great. Each profile's behavior is working as expected. I think nothing else needs to be added and should be ready for release. I will refactor this in future updates as well.

Thanks for the awesome work too! :)

[sunnyhaibin]

Hey eFini,

I just took another look of the logic we have and it seems like we might have the modes flipped. 🙂

So with the current code:

• The UI shows as Laneline, but uses Laneless/e2e model instead
• The UI shows as Laneless/e2e, but uses Laneline model instead

    # Turn off lanes during lane change
    if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
      self.LP.lll_prob *= self.lane_change_ll_prob
      self.LP.rll_prob *= self.lane_change_ll_prob
    if self.get_dlp_laneless_mode(): # Method returns TRUE - User/DLP wants to use Laneless/e2e
      # laneline logic 
      # Below executes Laneline model in lateral planner instead
      d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
      # Displays *Laneless* onroad instead
      self.laneless_mode_is_e2e = True # store auto mode value
    else:                            # Method returns FALSE - User/DLP wants to use Laneline
      # laneless logic
      # Below executes Laneless/e2e model in lateral planner instead
      d_path_xyz = self.path_xyz
      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
      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)
      # Displays *Laneline* onroad instead
      self.laneless_mode_is_e2e = False # store auto mode value

I flipped the logic to use and display the correct model and state when the method is called. Can you please take a look and let me know if it looks okay?

    # Turn off lanes during lane change
    if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
      self.LP.lll_prob *= self.lane_change_ll_prob
      self.LP.rll_prob *= self.lane_change_ll_prob
    if self.get_dlp_laneless_mode(): # Method returns TRUE - User/DLP wants to use Laneless/e2e
      # laneless logic
      # Below executes Laneless/e2e model in lateral planner correctly
      d_path_xyz = self.path_xyz
      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
      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)
      # Displays *Laneless* onroad correctly
      self.laneless_mode_is_e2e = True # store auto mode value
    else:                            # Method returns FALSE - User/DLP wants to use Laneline
      # laneline logic 
      # Below executes Laneline model in lateral planner correctly
      d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
      # Displays *Laneline* onroad correctly
      self.laneless_mode_is_e2e = False # store auto mode value

Thanks! :)

[sunnyhaibin]

Resolved.