From 7e87930a20fac9674a04f82aa76e8797fa8feab0 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 30 Jun 2021 16:19:39 -0500 Subject: [PATCH] Refactor lateral lag compensation (#21334) * add T_IDXS * refactor * fix test * unused * typo * needs casting * Update selfdrive/controls/lib/drive_helpers.py Co-authored-by: Adeeb Shihadeh * deprecate field * regen all * new segs * add todo * split back * clean * bad names * do in controls * add arg Co-authored-by: Adeeb Shihadeh old-commit-hash: 6838e1c82c5abb9f347228006219d96744b3fbb5 --- cereal | 2 +- selfdrive/controls/controlsd.py | 13 +- selfdrive/controls/lib/drive_helpers.py | 40 ++++- selfdrive/controls/lib/latcontrol_angle.py | 4 +- selfdrive/controls/lib/latcontrol_indi.py | 10 +- selfdrive/controls/lib/latcontrol_lqr.py | 8 +- selfdrive/controls/lib/latcontrol_pid.py | 6 +- selfdrive/controls/lib/lateral_planner.py | 47 ++--- selfdrive/controls/tests/test_lateral_mpc.py | 6 +- selfdrive/debug/mpc/live_lateral_mpc.py | 109 ------------ selfdrive/debug/mpc/live_longitudinal_mpc.py | 104 ----------- selfdrive/debug/mpc/longitudinal_mpc_model.py | 75 -------- selfdrive/debug/mpc/test_mpc_wobble.py | 115 ------------ selfdrive/debug/mpc/tune_longitudinal.py | 168 ------------------ selfdrive/modeld/constants.py | 12 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/process_replay/regen.py | 34 ++-- selfdrive/test/process_replay/regen_all.py | 21 +++ .../test/process_replay/test_processes.py | 25 ++- tools/replay/ui.py | 2 +- tools/sim/bridge.py | 2 +- 21 files changed, 144 insertions(+), 661 deletions(-) delete mode 100755 selfdrive/debug/mpc/live_lateral_mpc.py delete mode 100755 selfdrive/debug/mpc/live_longitudinal_mpc.py delete mode 100755 selfdrive/debug/mpc/longitudinal_mpc_model.py delete mode 100755 selfdrive/debug/mpc/test_mpc_wobble.py delete mode 100755 selfdrive/debug/mpc/tune_longitudinal.py create mode 100644 selfdrive/test/process_replay/regen_all.py diff --git a/cereal b/cereal index e1793a1854..e232a01457 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e1793a1854eb5f76a304b30ee96dee5a0f0b2cc4 +Subproject commit e232a014579ba3a45bdba1968502c11ae2005f1a diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9fbd919bd3..2ab761158e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -13,6 +13,7 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise +from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI @@ -454,7 +455,12 @@ class Controls: actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan) + desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, + lat_plan.psis, + lat_plan.curvatures, + lat_plan.curvatureRates) + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, + desired_curvature, desired_curvature_rate) else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0 and self.active: @@ -556,12 +562,8 @@ class Controls: # Curvature & Steering angle params = self.sm['liveParameters'] - lat_plan = self.sm['lateralPlan'] - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo) - angle_steers_des = math.degrees(self.VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) - angle_steers_des += params.angleOffsetDeg # controlsState dat = messaging.new_message('controlsState') @@ -580,7 +582,6 @@ class Controls: controlsState.enabled = self.enabled controlsState.active = self.active controlsState.curvature = curvature - controlsState.steeringAngleDesiredDeg = angle_steers_des controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ba433066d9..a3b5a62e12 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,15 +1,23 @@ +from cereal import car from common.numpy_fast import clip, interp +from common.realtime import DT_MDL from selfdrive.config import Conversions as CV -from cereal import car +from selfdrive.modeld.constants import T_IDXS + # kph V_CRUISE_MAX = 135 V_CRUISE_MIN = 8 V_CRUISE_DELTA = 8 V_CRUISE_ENABLE_MIN = 40 -MPC_N = 16 +LAT_MPC_N = 16 +CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 +# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla +MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992] +MAX_CURVATURE_RATE_SPEEDS = [0, 35] + class MPC_COST_LAT: PATH = 1.0 HEADING = 1.0 @@ -52,3 +60,31 @@ def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): return v_cruise_last return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + + +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): + if len(psis) != CONTROL_N: + psis = [0.0 for i in range(CONTROL_N)] + curvatures = [0.0 for i in range(CONTROL_N)] + curvature_rates = [0.0 for i in range(CONTROL_N)] + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + current_curvature = curvatures[0] + psi = interp(delay, T_IDXS[:CONTROL_N], psis) + desired_curvature_rate = curvature_rates[0] + + # MPC can plan to turn the wheel and turn back before t_delay. This means + # in high delay cases some corrections never even get commanded. So just use + # psi to calculate a simple linearization of desired curvature + curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature + desired_curvature = current_curvature + 2 * curvature_diff_from_psi + + max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES) + safe_desired_curvature_rate = clip(desired_curvature_rate, + -max_curvature_rate, + max_curvature_rate) + safe_desired_curvature = clip(desired_curvature, + current_curvature - max_curvature_rate/DT_MDL, + current_curvature + max_curvature_rate/DT_MDL) + return safe_desired_curvature, safe_desired_curvature_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index eab023b98b..a641687b4f 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -9,7 +9,7 @@ class LatControlAngle(): def reset(self): pass - def update(self, active, CS, CP, VM, params, lat_plan): + def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < 0.3 or not active: @@ -17,7 +17,7 @@ class LatControlAngle(): angle_steers_des = float(CS.steeringAngleDeg) else: angle_log.active = True - angle_steers_des = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) + angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des += params.angleOffsetDeg angle_log.saturated = False diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index ba39878952..6f234efdee 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -80,7 +80,7 @@ class LatControlINDI(): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, VM, params, lat_plan): + def update(self, active, CS, CP, VM, params, curvature, curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) @@ -91,15 +91,15 @@ class LatControlINDI(): indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) + steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo) + steers_des += math.radians(params.angleOffsetDeg) if CS.vEgo < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.delayed_output = 0.0 else: - steers_des = VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo) - steers_des += math.radians(params.angleOffsetDeg) - rate_des = VM.get_steer_from_curvature(-lat_plan.curvatureRate, CS.vEgo) + rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo) # Expected actuator value alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) @@ -142,4 +142,4 @@ class LatControlINDI(): check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) - return float(self.output_steer), 0, indi_log + return float(self.output_steer), float(steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 03464ea520..e445fb3f8a 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -1,10 +1,10 @@ import math import numpy as np -from selfdrive.controls.lib.drive_helpers import get_steer_max from common.numpy_fast import clip from common.realtime import DT_CTRL from cereal import log +from selfdrive.controls.lib.drive_helpers import get_steer_max class LatControlLQR(): @@ -44,7 +44,7 @@ class LatControlLQR(): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, VM, params, lat_plan): + def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) @@ -53,7 +53,7 @@ class LatControlLQR(): # Subtract offset. Zero angle should correspond to zero torque steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg - desired_angle = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) + desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg desired_angle += instant_offset # Only add offset that originates from vehicle model errors @@ -98,4 +98,4 @@ class LatControlLQR(): lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated - return output_steer, 0, lqr_log + return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 65f2c6fbfc..b258f9eded 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -15,12 +15,12 @@ class LatControlPID(): def reset(self): self.pid.reset() - def update(self, active, CS, CP, VM, params, lat_plan): + def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) - angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) + angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg if CS.vEgo < 0.3 or not active: @@ -48,4 +48,4 @@ class LatControlPID(): pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) - return output_steer, 0, pid_log + return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index f92c6c5e9f..d1575ca2ee 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -2,10 +2,10 @@ import os import math import numpy as np from common.realtime import sec_since_boot, DT_MDL -from common.numpy_fast import interp, clip +from common.numpy_fast import interp from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS +from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from selfdrive.config import Conversions as CV import cereal.messaging as messaging @@ -18,9 +18,6 @@ LOG_MPC = os.environ.get('LOG_MPC', False) LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. -# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla -MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992] -MAX_CURVATURE_RATE_SPEEDS = [0, 35] DESIRES = { LaneChangeDirection.none: { @@ -173,12 +170,12 @@ class LateralPlanner(): # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0]) self.libmpc.set_weights(path_cost, heading_cost, CP.steerRateCost) - y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) - heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) + y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) + heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts - assert len(y_pts) == MPC_N + 1 - assert len(heading_pts) == MPC_N + 1 + assert len(y_pts) == LAT_MPC_N + 1 + assert len(heading_pts) == LAT_MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, float(v_ego), CAR_ROTATION_RADIUS, @@ -188,29 +185,7 @@ class LateralPlanner(): self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 - self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature) - - # TODO this needs more thought, use .2s extra for now to estimate other delays - delay = CP.steerActuatorDelay + .2 - current_curvature = self.mpc_solution.curvature[0] - psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi) - next_curvature_rate = self.mpc_solution.curvature_rate[0] - - # MPC can plan to turn the wheel and turn back before t_delay. This means - # in high delay cases some corrections never even get commanded. So just use - # psi to calculate a simple linearization of desired curvature - curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature - next_curvature = current_curvature + 2 * curvature_diff_from_psi - - self.desired_curvature = next_curvature - self.desired_curvature_rate = next_curvature_rate - max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES) - self.safe_desired_curvature_rate = clip(self.desired_curvature_rate, - -max_curvature_rate, - max_curvature_rate) - self.safe_desired_curvature = clip(self.desired_curvature, - self.safe_desired_curvature - max_curvature_rate/DT_MDL, - self.safe_desired_curvature + max_curvature_rate/DT_MDL) + self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.mpc_solution.curvature) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) @@ -234,15 +209,13 @@ class LateralPlanner(): plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2']) plan_send.lateralPlan.laneWidth = float(self.LP.lane_width) plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts] + plan_send.lateralPlan.psis = [float(x) for x in self.mpc_solution.psi[0:CONTROL_N]] + plan_send.lateralPlan.curvatures = [float(x) for x in self.mpc_solution.curvature[0:CONTROL_N]] + plan_send.lateralPlan.curvatureRates = [float(x) for x in self.mpc_solution.curvature_rate[0:CONTROL_N-1]] +[0.0] plan_send.lateralPlan.lProb = float(self.LP.lll_prob) plan_send.lateralPlan.rProb = float(self.LP.rll_prob) plan_send.lateralPlan.dProb = float(self.LP.d_prob) - plan_send.lateralPlan.rawCurvature = float(self.desired_curvature) - plan_send.lateralPlan.rawCurvatureRate = float(self.desired_curvature_rate) - plan_send.lateralPlan.curvature = float(self.safe_desired_curvature) - plan_send.lateralPlan.curvatureRate = float(self.safe_desired_curvature_rate) - plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.lateralPlan.desire = self.desire diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index c27bdf2bdf..727384df1a 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -1,7 +1,7 @@ import unittest import numpy as np from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS +from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., @@ -14,8 +14,8 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., mpc_solution = libmpc_py.ffi.new("log_t *") - y_pts = poly_shift * np.ones(MPC_N + 1) - heading_pts = np.zeros(MPC_N + 1) + y_pts = poly_shift * np.ones(LAT_MPC_N + 1) + heading_pts = np.zeros(LAT_MPC_N + 1) cur_state = libmpc_py.ffi.new("state_t *") diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py deleted file mode 100755 index c85be58aa9..0000000000 --- a/selfdrive/debug/mpc/live_lateral_mpc.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 -import matplotlib -matplotlib.use('TkAgg') - -import sys -import cereal.messaging as messaging -import numpy as np -import matplotlib.pyplot as plt - -# debug liateral MPC by plotting its trajectory. To receive liveLongitudinalMpc packets, -# set on LOG_MPC env variable and run plannerd on a replay - - -def mpc_vwr_thread(addr="127.0.0.1"): - - plt.ion() - fig = plt.figure(figsize=(15, 20)) - ax = fig.add_subplot(131) - aa = fig.add_subplot(132, sharey=ax) - ap = fig.add_subplot(133, sharey=ax) - - ax.set_xlim([-10, 10]) - ax.set_ylim([0., 100.]) - aa.set_xlim([-20., 20]) - ap.set_xlim([-5, 5]) - - ax.set_xlabel('x [m]') - ax.set_ylabel('y [m]') - aa.set_xlabel('steer_angle [deg]') - ap.set_xlabel('asset angle [deg]') - ax.grid(True) - aa.grid(True) - ap.grid(True) - - path_x = np.arange(0, 100) - mpc_path_x = np.arange(0, 49) - - p_path_y = np.zeros(100) - - l_path_y = np.zeros(100) - r_path_y = np.zeros(100) - mpc_path_y = np.zeros(49) - mpc_steer_angle = np.zeros(49) - mpc_psi = np.zeros(49) - - line1, = ax.plot(mpc_path_y, mpc_path_x) - # line1b, = ax.plot(mpc_path_y, mpc_path_x, 'o') - - lineP, = ax.plot(p_path_y, path_x) - lineL, = ax.plot(l_path_y, path_x) - lineR, = ax.plot(r_path_y, path_x) - line3, = aa.plot(mpc_steer_angle, mpc_path_x) - line4, = ap.plot(mpc_psi, mpc_path_x) - ax.invert_xaxis() - aa.invert_xaxis() - plt.show() - - # *** log *** - livempc = messaging.sub_sock('liveMpc', addr=addr) - model = messaging.sub_sock('model', addr=addr) - path_plan_sock = messaging.sub_sock('lateralPlan', addr=addr) - - while 1: - lMpc = messaging.recv_sock(livempc, wait=True) - md = messaging.recv_sock(model) - pp = messaging.recv_sock(path_plan_sock) - - if md is not None: - p_poly = np.array(md.model.path.poly) - l_poly = np.array(md.model.leftLane.poly) - r_poly = np.array(md.model.rightLane.poly) - - p_path_y = np.polyval(p_poly, path_x) # lgtm[py/multiple-definition] - l_path_y = np.polyval(r_poly, path_x) - r_path_y = np.polyval(l_poly, path_x) - - if pp is not None: - p_path_y = np.polyval(pp.lateralPlan.dPolyDEPRECATED, path_x) - lineP.set_xdata(p_path_y) - lineP.set_ydata(path_x) - - if lMpc is not None: - mpc_path_x = list(lMpc.liveMpc.x)[1:] - mpc_path_y = list(lMpc.liveMpc.y)[1:] - mpc_steer_angle = list(lMpc.liveMpc.delta)[1:] - mpc_psi = list(lMpc.liveMpc.psi)[1:] - - line1.set_xdata(mpc_path_y) - line1.set_ydata(mpc_path_x) - lineL.set_xdata(l_path_y) - lineL.set_ydata(path_x) - lineR.set_xdata(r_path_y) - lineR.set_ydata(path_x) - line3.set_xdata(np.asarray(mpc_steer_angle)*180./np.pi * 14) - line3.set_ydata(mpc_path_x) - line4.set_xdata(np.asarray(mpc_psi)*180./np.pi) - line4.set_ydata(mpc_path_x) - - aa.relim() - aa.autoscale_view(True, scaley=True, scalex=True) - - fig.canvas.draw() - fig.canvas.flush_events() - -if __name__ == "__main__": - if len(sys.argv) > 1: - mpc_vwr_thread(sys.argv[1]) - else: - mpc_vwr_thread() diff --git a/selfdrive/debug/mpc/live_longitudinal_mpc.py b/selfdrive/debug/mpc/live_longitudinal_mpc.py deleted file mode 100755 index 4861a03ede..0000000000 --- a/selfdrive/debug/mpc/live_longitudinal_mpc.py +++ /dev/null @@ -1,104 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import cereal.messaging as messaging -import numpy as np -import matplotlib.pyplot as plt - -N = 21 - -# debug longitudinal MPC by plotting its trajectory. To receive liveLongitudinalMpc packets, -# set on LOG_MPC env variable and run plannerd on a replay - -def plot_longitudinal_mpc(addr="127.0.0.1"): - # *** log *** - livempc = messaging.sub_sock('liveLongitudinalMpc', addr=addr, conflate=True) - radarstate = messaging.sub_sock('radarState', addr=addr, conflate=True) - - plt.ion() - fig = plt.figure() - - t = np.hstack([np.arange(0.0, 0.8, 0.2), np.arange(0.8, 10.6, 0.6)]) - - p_x_ego = fig.add_subplot(3, 2, 1) - p_v_ego = fig.add_subplot(3, 2, 3) - p_a_ego = fig.add_subplot(3, 2, 5) - # p_x_l = fig.add_subplot(3, 2, 2) - # p_a_l = fig.add_subplot(3, 2, 6) - p_d_l = fig.add_subplot(3, 2, 2) - p_d_l_v = fig.add_subplot(3, 2, 4) - p_d_l_vv = fig.add_subplot(3, 2, 6) - - p_v_ego.set_ylim([0, 30]) - p_a_ego.set_ylim([-4, 4]) - p_d_l.set_ylim([-1, 10]) - - p_x_ego.set_title('x') - p_v_ego.set_title('v') - p_a_ego.set_title('a') - p_d_l.set_title('rel dist') - - l_x_ego, = p_x_ego.plot(t, np.zeros(N)) - l_v_ego, = p_v_ego.plot(t, np.zeros(N)) - l_a_ego, = p_a_ego.plot(t, np.zeros(N)) - l_x_l, = p_x_ego.plot(t, np.zeros(N)) - l_v_l, = p_v_ego.plot(t, np.zeros(N)) - l_a_l, = p_a_ego.plot(t, np.zeros(N)) - l_d_l, = p_d_l.plot(t, np.zeros(N)) - l_d_l_v, = p_d_l_v.plot(np.zeros(N)) - l_d_l_vv, = p_d_l_vv.plot(np.zeros(N)) - p_x_ego.legend(['ego', 'l']) - p_v_ego.legend(['ego', 'l']) - p_a_ego.legend(['ego', 'l']) - p_d_l_v.set_xlabel('d_rel') - p_d_l_v.set_ylabel('v_rel') - p_d_l_v.set_ylim([-20, 20]) - p_d_l_v.set_xlim([0, 100]) - p_d_l_vv.set_xlabel('d_rel') - p_d_l_vv.set_ylabel('v_rel') - p_d_l_vv.set_ylim([-5, 5]) - p_d_l_vv.set_xlim([10, 40]) - - while True: - lMpc = messaging.recv_sock(livempc, wait=True) - rs = messaging.recv_sock(radarstate, wait=True) - - if lMpc is not None: - - if lMpc.liveLongitudinalMpc.mpcId != 1: - continue - - x_ego = list(lMpc.liveLongitudinalMpc.xEgo) - v_ego = list(lMpc.liveLongitudinalMpc.vEgo) - a_ego = list(lMpc.liveLongitudinalMpc.aEgo) - x_l = list(lMpc.liveLongitudinalMpc.xLead) - v_l = list(lMpc.liveLongitudinalMpc.vLead) - # a_l = list(lMpc.liveLongitudinalMpc.aLead) - a_l = rs.radarState.leadOne.aLeadK * np.exp(-lMpc.liveLongitudinalMpc.aLeadTau * t**2 / 2) - #print(min(a_ego), lMpc.liveLongitudinalMpc.qpIterations) - - l_x_ego.set_ydata(x_ego) - l_v_ego.set_ydata(v_ego) - l_a_ego.set_ydata(a_ego) - - l_x_l.set_ydata(x_l) - l_v_l.set_ydata(v_l) - l_a_l.set_ydata(a_l) - - l_d_l.set_ydata(np.array(x_l) - np.array(x_ego)) - l_d_l_v.set_ydata(np.array(v_l) - np.array(v_ego)) - l_d_l_v.set_xdata(np.array(x_l) - np.array(x_ego)) - l_d_l_vv.set_ydata(np.array(v_l) - np.array(v_ego)) - l_d_l_vv.set_xdata(np.array(x_l) - np.array(x_ego)) - - p_x_ego.relim() - p_x_ego.autoscale_view(True, scaley=True, scalex=True) - fig.canvas.draw() - fig.canvas.flush_events() - - -if __name__ == "__main__": - if len(sys.argv) > 1: - plot_longitudinal_mpc(sys.argv[1]) - else: - plot_longitudinal_mpc() diff --git a/selfdrive/debug/mpc/longitudinal_mpc_model.py b/selfdrive/debug/mpc/longitudinal_mpc_model.py deleted file mode 100755 index 1a1c00c653..0000000000 --- a/selfdrive/debug/mpc/longitudinal_mpc_model.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np - -import matplotlib.pyplot as plt - -from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py - -libmpc = libmpc_py.libmpc - -dt = 1 -speeds = [6.109375, 5.9765625, 6.6367188, 7.6875, 8.7578125, 9.4375, 10.21875, 11.070312, 11.679688, 12.21875] -accelerations = [0.15405273, 0.39575195, 0.36669922, 0.29248047, 0.27856445, 0.27832031, 0.29736328, 0.22705078, 0.16003418, 0.15185547] -ts = [t * dt for t in range(len(speeds))] - -# TODO: Get from actual model packet -x = 0.0 -positions = [] -for v in speeds: - positions.append(x) - x += v * dt - - -# Polyfit trajectories -x_poly = list(map(float, np.polyfit(ts, positions, 3))) -v_poly = list(map(float, np.polyfit(ts, speeds, 3))) -a_poly = list(map(float, np.polyfit(ts, accelerations, 3))) - -x_poly = libmpc_py.ffi.new("double[4]", x_poly) -v_poly = libmpc_py.ffi.new("double[4]", v_poly) -a_poly = libmpc_py.ffi.new("double[4]", a_poly) - -cur_state = libmpc_py.ffi.new("state_t *") -cur_state[0].x_ego = 0 -cur_state[0].v_ego = 10 -cur_state[0].a_ego = 0 - -libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0) - -mpc_solution = libmpc_py.ffi.new("log_t *") -libmpc.init_with_simulation(cur_state[0].v_ego) - -libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly) - -# Converge to solution -for _ in range(10): - libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly) - - -ts_sol = list(mpc_solution[0].t) -x_sol = list(mpc_solution[0].x_ego) -v_sol = list(mpc_solution[0].v_ego) -a_sol = list(mpc_solution[0].a_ego) - - -plt.figure() -plt.subplot(3, 1, 1) -plt.plot(ts, positions, 'k--') -plt.plot(ts_sol, x_sol) -plt.ylabel('Position [m]') -plt.xlabel('Time [s]') - -plt.subplot(3, 1, 2) -plt.plot(ts, speeds, 'k--') -plt.plot(ts_sol, v_sol) -plt.xlabel('Time [s]') -plt.ylabel('Speed [m/s]') - -plt.subplot(3, 1, 3) -plt.plot(ts, accelerations, 'k--') -plt.plot(ts_sol, a_sol) - -plt.xlabel('Time [s]') -plt.ylabel('Acceleration [m/s^2]') - -plt.show() diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py deleted file mode 100755 index ee74c2cf13..0000000000 --- a/selfdrive/debug/mpc/test_mpc_wobble.py +++ /dev/null @@ -1,115 +0,0 @@ -#! /usr/bin/env python -# type: ignore -import matplotlib.pyplot as plt -from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS -import math - -libmpc = libmpc_py.libmpc -libmpc.init() -libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.) - -cur_state = libmpc_py.ffi.new("state_t *") -cur_state[0].x = 0.0 -cur_state[0].y = 0.0 -cur_state[0].psi = 0.0 -cur_state[0].delta = 0.0 - -mpc_solution = libmpc_py.ffi.new("log_t *") -xx = [] -yy = [] -deltas = [] -psis = [] -times = [] - -curvature_factor = 0.3 -v_ref = 1.0 * 20.12 # 45 mph - -for i in range(1): - cur_state[0].delta = math.radians(510. / 13.) - libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref], - curvature_factor, CAR_ROTATION_RADIUS, - [0.0]*MPC_N, [0.0]*MPC_N) - -timesi = [] -ct = 0 -for i in range(MPC_N + 1): - timesi.append(ct) - if i <= 4: - ct += 0.05 - else: - ct += 0.15 - - -xi = list(mpc_solution[0].x) -yi = list(mpc_solution[0].y) -psii = list(mpc_solution[0].psi) -deltai = list(mpc_solution[0].delta) -print("COST: ", mpc_solution[0].cost) - - -plt.figure(0) -plt.subplot(3, 1, 1) -plt.plot(timesi, psii) -plt.ylabel('psi') -plt.grid(True) -plt.subplot(3, 1, 2) -plt.plot(timesi, deltai) -plt.ylabel('delta') -plt.grid(True) -plt.subplot(3, 1, 3) -plt.plot(timesi, yi) -plt.ylabel('y') -plt.grid(True) -plt.show() - - -#### UNCOMMENT TO CHECK ITERATIVE SOLUTION -#### -####for i in range(100): -#### libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, -#### curvature_factor, v_ref, LANE_WIDTH) -#### print "x", list(mpc_solution[0].x) -#### print "y", list(mpc_solution[0].y) -#### print "delta", list(mpc_solution[0].delta) -#### print "psi", list(mpc_solution[0].psi) -#### # cur_state[0].x = mpc_solution[0].x[1] -#### # cur_state[0].y = mpc_solution[0].y[1] -#### # cur_state[0].psi = mpc_solution[0].psi[1] -#### cur_state[0].delta = radians(200 / 13.)#mpc_solution[0].delta[1] -#### -#### xx.append(cur_state[0].x) -#### yy.append(cur_state[0].y) -#### psis.append(cur_state[0].psi) -#### deltas.append(cur_state[0].delta) -#### times.append(i * 0.05) -#### -#### -####def f(x): -#### return p_poly[0] * x**3 + p_poly[1] * x**2 + p_poly[2] * x + p_poly[3] -#### -#### -##### planned = map(f, xx) -##### plt.figure(1) -##### plt.plot(yy, xx, 'r-') -##### plt.plot(planned, xx, 'b--', linewidth=0.5) -##### plt.axes().set_aspect('equal', 'datalim') -##### plt.gca().invert_xaxis() -#### -##### planned = map(f, map(float, list(mpc_solution[0].x)[1:])) -##### plt.figure(1) -##### plt.plot(map(float, list(mpc_solution[0].y)[1:]), map(float, list(mpc_solution[0].x)[1:]), 'r-') -##### plt.plot(planned, map(float, list(mpc_solution[0].x)[1:]), 'b--', linewidth=0.5) -##### plt.axes().set_aspect('equal', 'datalim') -##### plt.gca().invert_xaxis() -#### -####plt.figure(2) -####plt.subplot(2, 1, 1) -####plt.plot(times, psis) -####plt.ylabel('psi') -####plt.subplot(2, 1, 2) -####plt.plot(times, deltas) -####plt.ylabel('delta') -#### -#### -####plt.show() diff --git a/selfdrive/debug/mpc/tune_longitudinal.py b/selfdrive/debug/mpc/tune_longitudinal.py deleted file mode 100755 index f7fd43c48d..0000000000 --- a/selfdrive/debug/mpc/tune_longitudinal.py +++ /dev/null @@ -1,168 +0,0 @@ -#! /usr/bin/env python -# type: ignore -import numpy as np -import matplotlib.pyplot as plt -from selfdrive.controls.lib.longitudinal_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG - -# plot liongitudinal MPC trajectory by defining boundary conditions: -# ego and lead vehicles state. Use this script to tune MPC costs - -def RW(v_ego, v_l): - TR = 1.8 - G = 9.81 - return (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) - - -def NORM_RW_ERROR(v_ego, v_l, p): - return (RW(v_ego, v_l) + 4.0 - p) - #return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1) - - -v_ego = 20.0 -a_ego = 0 - -x_lead = 10.0 -v_lead = 20.0 -a_lead = -3.0 -a_lead_tau = 0. - -# v_ego = 7.02661012716 -# a_ego = -1.26143024772 - -# x_lead = 29.625 + 20 -# v_lead = 0.725235462189 + 1 -# a_lead = -1.00025629997 - -# a_lead_tau = 2.90729817665 - -#min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2) -min_a_lead_tau = 0.0 - -print(a_lead_tau, min_a_lead_tau) -a_lead_tau = max(a_lead_tau, min_a_lead_tau) - -ffi, libmpc = libmpc_py.get_libmpc(1) -libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) -libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau) - -cur_state = ffi.new("state_t *") -cur_state[0].x_ego = 0.0 -cur_state[0].v_ego = v_ego -cur_state[0].a_ego = a_ego -cur_state[0].x_l = x_lead -cur_state[0].v_l = v_lead - -mpc_solution = ffi.new("log_t *") - -for _ in range(10): - print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead)) - - -for i in range(21): - print("t: %.2f\t x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t" % (mpc_solution[0].t[i], mpc_solution[0].x_ego[i], mpc_solution[0].v_ego[i], mpc_solution[0].a_ego[i])) - print("x_l: %.2f\t v_l: %.2f\t \t" % (mpc_solution[0].x_l[i], mpc_solution[0].v_l[i])) - -t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)]) - -print(map(float, mpc_solution[0].x_ego)[-1]) -print(map(float, mpc_solution[0].x_l)[-1] - map(float, mpc_solution[0].x_ego)[-1]) - -plt.figure(figsize=(8, 8)) - -plt.subplot(4, 1, 1) -x_l = np.array(map(float, mpc_solution[0].x_l)) -plt.plot(t, map(float, mpc_solution[0].x_ego)) -plt.plot(t, x_l) -plt.legend(['ego', 'lead']) -plt.title('x') -plt.grid() - -plt.subplot(4, 1, 2) -v_ego = np.array(map(float, mpc_solution[0].v_ego)) -v_l = np.array(map(float, mpc_solution[0].v_l)) -plt.plot(t, v_ego) -plt.plot(t, v_l) -plt.legend(['ego', 'lead']) -plt.ylim([-1, max(max(v_ego), max(v_l))]) -plt.title('v') -plt.grid() - -plt.subplot(4, 1, 3) -plt.plot(t, map(float, mpc_solution[0].a_ego)) -plt.plot(t, map(float, mpc_solution[0].a_l)) -plt.legend(['ego', 'lead']) -plt.title('a') -plt.grid() - - -plt.subplot(4, 1, 4) -d_l = np.array(map(float, mpc_solution[0].x_l)) - np.array(map(float, mpc_solution[0].x_ego)) -desired = 4.0 + RW(v_ego, v_l) - -plt.plot(t, d_l) -plt.plot(t, desired, '--') -plt.ylim(-1, max(max(desired), max(d_l))) -plt.legend(['relative distance', 'desired distance']) -plt.grid() - -plt.show() - -# c1 = np.exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) -# c2 = np.exp(4.5 - d_l) -# print(c1) -# print(c2) - -# plt.figure() -# plt.plot(t, c1, label="NORM_RW_ERROR") -# plt.plot(t, c2, label="penalty function") -# plt.legend() - -# ## OLD MPC -# a_lead_tau = 1.5 -# a_lead_tau = max(a_lead_tau, -a_lead / (v_lead + 0.01)) - -# ffi, libmpc = libmpc_py.get_libmpc(1) -# libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) -# libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau) - -# cur_state = ffi.new("state_t *") -# cur_state[0].x_ego = 0.0 -# cur_state[0].v_ego = v_ego -# cur_state[0].a_ego = a_ego -# cur_state[0].x_lead = x_lead -# cur_state[0].v_lead = v_lead -# cur_state[0].a_lead = a_lead - -# mpc_solution = ffi.new("log_t *") - -# for _ in range(10): -# print libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau) - -# t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)]) - -# print(map(float, mpc_solution[0].x_ego)[-1]) -# print(map(float, mpc_solution[0].x_lead)[-1] - map(float, mpc_solution[0].x_ego)[-1]) -# plt.subplot(4, 2, 2) -# plt.plot(t, map(float, mpc_solution[0].x_ego)) -# plt.plot(t, map(float, mpc_solution[0].x_lead)) -# plt.legend(['ego', 'lead']) -# plt.title('x') - -# plt.subplot(4, 2, 4) -# plt.plot(t, map(float, mpc_solution[0].v_ego)) -# plt.plot(t, map(float, mpc_solution[0].v_lead)) -# plt.legend(['ego', 'lead']) -# plt.title('v') - -# plt.subplot(4, 2, 6) -# plt.plot(t, map(float, mpc_solution[0].a_ego)) -# plt.plot(t, map(float, mpc_solution[0].a_lead)) -# plt.legend(['ego', 'lead']) -# plt.title('a') - - -# plt.subplot(4, 2, 8) -# plt.plot(t, np.array(map(float, mpc_solution[0].x_lead)) - np.array(map(float, mpc_solution[0].x_ego))) - -# plt.show() diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 2bf48574f8..aa21cd9106 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -1,6 +1,8 @@ -MAX_DISTANCE = 140. -LANE_OFFSET = 1.8 -MAX_REL_V = 10. +import numpy as np +IDX_N = 33 -LEAD_X_SCALE = 10 -LEAD_Y_SCALE = 10 +def index_function(idx, max_val=192): + return (max_val/1024)*(idx**2) + + +T_IDXS = np.array([index_function(idx, max_val=10.0) for idx in range(IDX_N)], dtype=np.float64) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d43667d632..7b8f697bbe 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1ac9a43631a3d6c7316220897ab17f33a66bb05f \ No newline at end of file +999749c3955d504712564db3faf541f8c21c069d \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 2dfdba9e50..10c6c9815e 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -1,10 +1,9 @@ #!/usr/bin/env python3 -import argparse import os import time import multiprocessing from tqdm import tqdm - +import argparse # run DM procs os.environ["USE_WEBCAM"] = "1" @@ -112,6 +111,8 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = car_fingerprint + #TODO: init car, make sure starts engaged when segment is engaged + fake_daemons = { 'sensord': [ multiprocessing.Process(target=replay_service, args=('sensorEvents', lr)), @@ -165,22 +166,29 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): r = params.get("CurrentRoute", encoding='utf-8') return os.path.join(outdir, r + "--0") -if __name__ == "__main__": - - parser = argparse.ArgumentParser(description="Generate new segments from old ones") - parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") - parser.add_argument("route", type=str, help="The source route") - parser.add_argument("seg", type=int, help="Segment in source route") - args = parser.parse_args() - r = Route(args.route) - lr = LogReader(r.log_paths()[args.seg]) - fr = FrameReader(r.camera_paths()[args.seg]) +def regen_and_save(route, sidx, upload=False, use_route_meta=True): + if use_route_meta: + r = Route(args.route) + lr = LogReader(r.log_paths()[args.seg]) + fr = FrameReader(r.camera_paths()[args.seg]) + else: + lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") + fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") rpath = regen_segment(lr, {'roadCameraState': fr}) relr = os.path.relpath(rpath) print("\n\n", "*"*30, "\n\n") print("New route:", relr, "\n") - if args.upload: + if upload: upload_route(relr) + return relr + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Generate new segments from old ones") + parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") + parser.add_argument("route", type=str, help="The source route") + parser.add_argument("seg", type=int, help="Segment in source route") + args = parser.parse_args() + regen_and_save(args.route, args.seg, args.upload) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py new file mode 100644 index 0000000000..047c83f465 --- /dev/null +++ b/selfdrive/test/process_replay/regen_all.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +from selfdrive.test.process_replay.regen import regen_and_save +from selfdrive.test.process_replay.test_processes import original_segments as segments + +if __name__ == "__main__": + new_segments = [] + for segment in segments: + route = segment[1].rsplit('--', 1)[0] + sidx = int(segment[1].rsplit('--', 1)[1]) + relr = regen_and_save(route, sidx, upload=True, use_route_meta=False) + + print("\n\n", "*"*30, "\n\n") + print("New route:", relr, "\n") + relr = relr.replace('/', '|') + new_segments.append(f'("{segment[0]}", "{relr}"), ') + print() + print() + print() + print('COPY THIS INTO test_processes.py') + for seg in new_segments: + print(seg) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 504439a03b..a2bd753b41 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -11,16 +11,16 @@ from selfdrive.test.process_replay.process_replay import CONFIGS, replay_process from tools.lib.logreader import LogReader -segments = [ +original_segments = [ ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) - ("HONDA", "0982d79ebb0de295|2021-01-08--10-13-10--6"), # HONDA.CIVIC (NIDEC) - ("HONDA2", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"), # HONDA.ACCORD (BOSCH) - ("CHRYSLER", "52d86230ee29aa84|2021-01-10--17-16-34--30"), # CHRYSLER.PACIFICA + ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) + ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) + ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA - ("GM", "ae3ed0eb20960a20|2021-01-15--15-04-06--8"), # GM.VOLT - ("NISSAN", "e4d79cf6b8b19a0d|2021-01-17--14-48-08--7"), # NISSAN.XTRAIL + ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT + ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL ("VOLKSWAGEN", "ef895f46af5fd73f|2021-05-22--14-06-35--6"), # VW.AUDI_A3_MK3 # Enable when port is tested and dascamOnly is no longer set @@ -28,6 +28,19 @@ segments = [ #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS ] +segments = [ + ("HYUNDAI", "process_replay|fakedata|2021-06-30--01-00-31--0"), + ("TOYOTA", "process_replay|fakedata|2021-06-30--01-03-53--0"), + ("TOYOTA2", "process_replay|fakedata|2021-06-30--01-07-24--0"), + ("HONDA", "process_replay|fakedata|2021-06-30--01-48-33--0"), + ("HONDA2", "process_replay|fakedata|2021-06-30--01-52-56--0"), + ("CHRYSLER", "process_replay|fakedata|2021-06-30--01-23-40--0"), + ("SUBARU", "process_replay|fakedata|2021-06-30--01-27-22--0"), + ("GM", "process_replay|fakedata|2021-06-30--01-30-49--0"), + ("NISSAN", "process_replay|fakedata|2021-06-30--01-34-20--0"), + ("VOLKSWAGEN", "process_replay|fakedata|2021-06-30--01-37-52--0"), +] + # dashcamOnly makes don't need to be tested until a full port is done excluded_interfaces = ["mock", "ford", "mazda", "tesla"] diff --git a/tools/replay/ui.py b/tools/replay/ui.py index f3a831a664..611307847a 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -143,7 +143,7 @@ def ui_thread(addr, frame_address): plot_arr[:-1] = plot_arr[1:] plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg - plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['controlsState'].steeringAngleDesiredDeg + plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 4adcfce7d7..49082dd1a0 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -297,7 +297,7 @@ def bridge(q): sm.update(0) throttle_op = sm['carControl'].actuators.gas #[0,1] brake_op = sm['carControl'].actuators.brake #[0,1] - steer_op = sm['controlsState'].steeringAngleDesiredDeg # degrees [-180,180] + steer_op = sm['carControl'].actuators.steeringAngleDeg throttle_out = throttle_op steer_out = steer_op