mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-23 15:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

fade ll out and in (#1246)

Browse Source 
* fade ll out and in

* bug fixes

* pretty important fixes

* safer in case model misses desire input

* Safer float compares

Co-authored-by: Willem Melching <willem.melching@gmail.com>
This commit is contained in:
HaraldSchafer
2020-03-16 16:19:01 -07:00
committed by GitHub
parent 9a2066fc3a
commit af5f99d7cf
1 changed files with 21 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/pathplanner.py
+21 -5
View File
@@ -22,20 +22,24 @@ DESIRES = {
LaneChangeDirection.none: {
LaneChangeState.off: log.PathPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none,
},
LaneChangeDirection.left: {
LaneChangeState.off: log.PathPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft,
},
LaneChangeDirection.right: {
LaneChangeState.off: log.PathPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight,
},
}
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
states[0].x = v_ego * delay
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
@@ -55,6 +59,7 @@ class PathPlanner():
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
def setup_mpc(self):
@@ -110,19 +115,30 @@ class PathPlanner():
# 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
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out lanelines over 1s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.5:
if one_blinker:
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
else:
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
@@ -136,8 +152,8 @@ class PathPlanner():
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob = 0.
self.LP.r_prob = 0.
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)