fade ll out and in (#1246)
* 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:
parent
9a2066fc3a
commit
af5f99d7cf
|
@ -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)
|
||||
|
|
Loading…
Reference in a new issue