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

Nudgeless lane change

Browse Source 
Added toggles for nudgeless lane changes, lane detection, and one lane change per signal activation with a lane change delay factor.
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 6266c08db4
commit a8387af5f0
9 changed files with 92 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
selfdrive/controls/lib/desire_helper.py
View File

@@ -45,6 +45,10 @@ class DesireHelper:
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.lane_change_completed = False
self.lane_change_wait_timer = 0
self.update_frogpilot_params()
def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan):
@@ -52,6 +56,13 @@ class DesireHelper:
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if not (self.lane_detection and one_blinker) or below_lane_change_speed:
lane_available = True
else:
desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight
# Check if the lane width exceeds the threshold
lane_available = desired_lane >= self.lane_detection_width
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
@@ -60,6 +71,7 @@ class DesireHelper:
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.lane_change_wait_timer = 0
# LaneChangeState.preLaneChange
elif self.lane_change_state == LaneChangeState.preLaneChange:
@@ -74,10 +86,18 @@ class DesireHelper:
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
# Conduct a nudgeless lane change if all the conditions are met
self.lane_change_wait_timer += DT_MDL
if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay:
self.lane_change_wait_timer = 0
torque_applied = True
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
elif torque_applied and not blindspot_detected:
# Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on
self.lane_change_completed = self.one_lane_change
self.lane_change_state = LaneChangeState.laneChangeStarting
# LaneChangeState.laneChangeStarting
@@ -108,6 +128,9 @@ class DesireHelper:
self.prev_one_blinker = one_blinker
# Reset the flags
self.lane_change_completed &= one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Send keep pulse once per second during LaneChangeStart.preLaneChange
@@ -125,3 +148,9 @@ class DesireHelper:
def update_frogpilot_params(self):
is_metric = self.params.get_bool("IsMetric")
self.nudgeless = self.params.get_bool("NudgelessLaneChange")
self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0
self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection")
self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0
self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange")