Skip to content

Add green-light launch #38304

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
tfpgh wants to merge 1 commit into
base: master
Choose a base branch
from
+21 −1
Open

Add green-light launch #38304

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 21 additions & 1 deletion openpilot/selfdrive/controls/lib/longitudinal_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,11 @@
ALLOW_THROTTLE_THRESHOLD = 0.4
MIN_ALLOW_THROTTLE_SPEED = 2.5

LAUNCH_DISARM_SPEED = 2.0
LAUNCH_COMMIT_T = 3.5
LAUNCH_MOVING_SPEED = 1.2
LAUNCH_MAX_ACCEL = 1.5

# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
Expand Down Expand Up @@ -58,6 +63,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
self.output_a_target = 0.0
self.output_should_stop = False
self.launch_armed = False

self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
Expand Down Expand Up @@ -116,7 +122,7 @@ def update(self, sm):

# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
_, _, _, _, throttle_prob = self.parse_model(sm['modelV2'])
_, model_v, model_a, _, throttle_prob = self.parse_model(sm['modelV2'])
# Don't clip at low speeds since throttle_prob doesn't account for creep
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED

Expand Down Expand Up @@ -152,6 +158,20 @@ def update(self, sm):
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop

if sm['carState'].standstill:
self.launch_armed = True
elif v_ego > LAUNCH_DISARM_SPEED:
self.launch_armed = False
if (self.launch_armed and sm['selfdriveState'].experimentalMode and not output_should_stop_e2e and
np.interp(LAUNCH_COMMIT_T, T_IDXS_MPC, model_v) > LAUNCH_DISARM_SPEED):
t_cut = min(float(T_IDXS_MPC[np.argmax(model_v > LAUNCH_MOVING_SPEED)]), LAUNCH_COMMIT_T)
t_shifted = T_IDXS_MPC + t_cut
v_shifted = np.interp(t_shifted, T_IDXS_MPC, model_v)
a_shifted = np.interp(t_shifted, T_IDXS_MPC, model_a)
a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t, vEgoStopping=self.CP.vEgoStopping)[0]
a_launch_max = np.interp(v_ego, [LAUNCH_MOVING_SPEED, LAUNCH_DISARM_SPEED], [LAUNCH_MAX_ACCEL, 0.])
output_a_target_e2e = max(output_a_target_e2e, min(a_launch, a_launch_max))

if sm['selfdriveState'].experimentalMode:
output_a_target = min(output_a_target_e2e, output_a_target_mpc)
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
Expand Down
Loading