brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Blind spot path

Browse Source 
Added toggle to display a red path in the adjacent lane when a vehicle is detected in the blind spot.
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 0cd83a9021
commit e20aaf75e5
10 changed files with 78 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -2,6 +2,7 @@ import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
@@ -44,6 +45,15 @@ class FrogPilotPlanner:
self.accel_limits = [min_accel, max_accel]
# Update the current lane widths
check_lane_width = self.blind_spot_path
if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN:
self.lane_width_left = float(self.fpf.calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
self.lane_width_right = float(self.fpf.calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
else:
self.lane_width_left = 0
self.lane_width_right = 0
# Update the max allowed speed
self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego)
@@ -65,12 +75,16 @@ class FrogPilotPlanner:
frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
frogpilotPlan = frogpilot_plan_send.frogpilotPlan
frogpilotPlan.laneWidthLeft = self.lane_width_left
frogpilotPlan.laneWidthRight = self.lane_width_right
pm.send('frogpilotPlan', frogpilot_plan_send)
def update_frogpilot_params(self, params):
self.is_metric = params.get_bool("IsMetric")
custom_ui = params.get_bool("CustomUI")
self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
longitudinal_tune = params.get_bool("LongitudinalTune")
self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0