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

Use turn desires when below the minimum lane change speed

Browse Source 
Added toggle to use turn desires when below the minimum lane change speed for more precise turns.
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 28a898f874
commit cfbb472acd
8 changed files with 63 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,6 +5,7 @@ from openpilot.common.realtime import DT_MDL
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
TurnDirection = log.Desire
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
@@ -30,6 +31,12 @@ DESIRES = {
},
}
TURN_DESIRES = {
TurnDirection.none: log.Desire.none,
TurnDirection.turnLeft: log.Desire.turnLeft,
TurnDirection.turnRight: log.Desire.turnRight,
}
class DesireHelper:
def __init__(self):
@@ -45,7 +52,10 @@ class DesireHelper:
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.turn_direction = TurnDirection.none
self.lane_change_completed = False
self.turn_completed = False
self.lane_change_wait_timer = 0
@@ -66,7 +76,15 @@ class DesireHelper:
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
self.turn_direction = TurnDirection.none
elif one_blinker and below_lane_change_speed and self.turn_desires:
self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
# Set the "turn_completed" flag to prevent lane changes after completing a turn
self.turn_completed = True
else:
# TurnDirection.turnLeft / turnRight
self.turn_direction = TurnDirection.none
# LaneChangeState.off
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
@@ -130,8 +148,14 @@ class DesireHelper:
# Reset the flags
self.lane_change_completed &= one_blinker
self.turn_completed &= one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
if self.turn_direction != TurnDirection.none:
self.desire = TURN_DESIRES[self.turn_direction]
elif not self.turn_completed:
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
else:
self.desire = log.Desire.none
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
@@ -154,3 +178,5 @@ class DesireHelper:
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")
self.turn_desires = self.params.get_bool("TurnDesires")