mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 01:52:06 +08:00
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 <adeebshihadeh@gmail.com> * deprecate field * regen all * new segs * add todo * split back * clean * bad names * do in controls * add arg Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> old-commit-hash: 6838e1c82c5abb9f347228006219d96744b3fbb5
This commit is contained in:
HaraldSchafer
+1
-1
Submodule cereal updated: e1793a1854...e232a01457
@@ -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
|
||||
|
||||
@@ -1,15 +1,23 @@
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
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 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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 *")
|
||||
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +1 @@
|
||||
1ac9a43631a3d6c7316220897ab17f33a66bb05f
|
||||
999749c3955d504712564db3faf541f8c21c069d
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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"]
|
||||
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user