diff --git a/examples/longitudinal_profiles.py b/examples/longitudinal_profiles.py index f9f4c2f1a9..6d0b6d2d44 100755 --- a/examples/longitudinal_profiles.py +++ b/examples/longitudinal_profiles.py @@ -13,7 +13,7 @@ from pathlib import Path from cereal import messaging, car from opendbc.car.structs import CarControl from opendbc.car.common.conversions import Conversions -from openpilot.common.realtime import DT_CTRL, Ratekeeper +from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N @@ -70,25 +70,25 @@ MANEUVERS = [ # ), Maneuver( "brake step response: -1m/ss from 20mph", - [Action(0, 2), Action(-1, 3)], + [Action(-1, 3)], repeat=3, initial_speed=20. * Conversions.MPH_TO_MS, ), Maneuver( "brake step response: -4m/ss from 20mph", - [Action(0, 2), Action(-4, 3)], + [Action(-4, 3)], repeat=3, initial_speed=20. * Conversions.MPH_TO_MS, ), Maneuver( "gas step response: +1m/ss from 20mph", - [Action(0, 2), Action(1, 3)], + [Action(1, 3)], repeat=3, initial_speed=20. * Conversions.MPH_TO_MS, ), Maneuver( "gas step response: +4m/ss from 20mph", - [Action(0, 2), Action(4, 3)], + [Action(4, 3)], repeat=3, initial_speed=20. * Conversions.MPH_TO_MS, ), @@ -156,7 +156,9 @@ def main(): maneuvers = iter(MANEUVERS) maneuver = None + ready_cnt = 0 repeat = 0 + maneuver_active = False while True: sm.update() @@ -166,7 +168,7 @@ def main(): if maneuver is None: print('We are done!') - print(maneuver) + # print(maneuver) if maneuver is not None: pass @@ -177,17 +179,39 @@ def main(): longitudinalPlan = plan_send.longitudinalPlan if maneuver is not None: + # desired_ + v_ego = sm['carState'].vEgo - diff = maneuver.initial_speed - v_ego - longitudinalPlan.speeds = [(v_ego + min(i / (CONTROL_N - 1), 1) * diff) for i in range(CONTROL_N)] - longitudinalPlan.accels = [(maneuver.initial_speed - sm['carState'].vEgo) * 0.8] * CONTROL_N - longitudinalPlan.jerks = [0] * CONTROL_N - print('v_ego', sm['carState'].vEgo) - print('speeds:', longitudinalPlan.speeds) - print('accels:', longitudinalPlan.accels) + cs = sm['carState'] + + ready = abs(v_ego - maneuver.initial_speed) < 0.4 and cs.cruiseState.enabled and not cs.cruiseState.standstill + ready_cnt = (ready_cnt + 1) if ready else 0 + if ready_cnt > (2. / DT_MDL): + print('ready!!!') + maneuver_active = True + + if maneuver_active: + accel = maneuver.actions[0].accel + diff = v_ego * - v_ego + # longitudinalPlan.speeds = [(v_ego + min(i / (CONTROL_N - 1), 1) * diff) for i in range(CONTROL_N)] + longitudinalPlan.speeds = [v_ego + accel for i in range(CONTROL_N)] + longitudinalPlan.accels = [accel] * CONTROL_N + longitudinalPlan.jerks = [0] * CONTROL_N + print('v_ego', sm['carState'].vEgo) + print('speeds:', longitudinalPlan.speeds) + print('accels:', longitudinalPlan.accels) + else: + accel = (maneuver.initial_speed - cs.vEgo) + diff = maneuver.initial_speed - v_ego + longitudinalPlan.speeds = [(v_ego + min(i / (CONTROL_N - 1), 1) * diff) for i in range(CONTROL_N)] + longitudinalPlan.accels = [accel] * CONTROL_N + longitudinalPlan.jerks = [0] * CONTROL_N + # print('v_ego', sm['carState'].vEgo) + # print('speeds:', longitudinalPlan.speeds) + # print('accels:', longitudinalPlan.accels) a_target, should_stop = get_accel_from_plan(CP, longitudinalPlan.speeds, longitudinalPlan.accels) - longitudinalPlan.aTarget = a_target + longitudinalPlan.aTarget = accel longitudinalPlan.shouldStop = should_stop print('aTarget:', longitudinalPlan.aTarget) longitudinalPlan.allowBrake = True