mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 08:14:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remember lane change direction when blinker turns off

Browse Source 
old-commit-hash: fe9ccb27b1
This commit is contained in:
Willem Melching
2020-01-27 16:19:22 -08:00
parent 903c64c9bf
commit f00a8a1908
1 changed files with 11 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/pathplanner.py
+11 -10
View File
@@ -55,6 +55,7 @@ class PathPlanner():
self.solution_invalid_cnt = 0 self.solution_invalid_cnt = 0
self.path_offset_i = 0.0 self.path_offset_i = 0.0
self.lane_change_state = LaneChangeState.off self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0 self.lane_change_timer = 0.0
self.prev_one_blinker = False self.prev_one_blinker = False
@@ -89,21 +90,21 @@ class PathPlanner():
self.LP.parse_model(sm['model']) self.LP.parse_model(sm['model'])
# Lane change logic # Lane change logic
lane_change_direction = LaneChangeDirection.none
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else: else:
if sm['carState'].leftBlinker:
lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
lane_change_direction = LaneChangeDirection.right
torque_applied = sm['carState'].steeringPressed and \ torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \
(sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right)) (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
@@ -137,7 +138,7 @@ class PathPlanner():
self.prev_one_blinker = one_blinker self.prev_one_blinker = one_blinker
desire = DESIRES[lane_change_direction][self.lane_change_state] desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change # Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
@@ -215,7 +216,7 @@ class PathPlanner():
plan_send.pathPlan.desire = desire plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = lane_change_direction plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
pm.send('pathPlan', plan_send) pm.send('pathPlan', plan_send)