def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
# TODO: Read from car params at runtime
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
learner = ParamsLearner(CP)
while True:
sm.update()
for which, updated in sm.updated.items():
if not updated:
continue
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
# TODO: set valid to false when locationd stops sending
# TODO: make sure controlsd knows when there is no gyro
# TODO: move posenetValid somewhere else to show the model uncertainty alert
# TODO: Save and resume values from param
# TODO: Change KF to allow mass, etc to be inputs in predict step
if sm.updated['carState']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.valid = True # TODO: Check if learned values are sane
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(
x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = math.degrees(
x[States.ANGLE_OFFSET_FAST])
# P = learner.kf.P
# print()
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
# print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
# print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
# print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
pm.send('liveParameters', msg)
def test_steering_ipas(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
params.enableApgs = True
CI = CarInterface(params, CarController)
CI.CC.angle_control = True
# Get parser
parser_signals = [
('SET_ME_X10', 'STEERING_IPAS', 0),
('SET_ME_X40', 'STEERING_IPAS', 0),
('ANGLE', 'STEERING_IPAS', 0),
('STATE', 'STEERING_IPAS', 0),
('DIRECTION_CMD', 'STEERING_IPAS', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for enabled in [True, False]:
for steer in np.linspace(-510., 510., 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.steerAngle = float(steer)
control.enabled = enabled
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(1000 if steer < -505 else 25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
parser.update(int(sec_since_boot() * 1e9), False)
self.assertEqual(0x10, parser.vl['STEERING_IPAS']['SET_ME_X10'])
self.assertEqual(0x40, parser.vl['STEERING_IPAS']['SET_ME_X40'])
expected_state = 3 if enabled else 1
self.assertEqual(expected_state, parser.vl['STEERING_IPAS']['STATE'])
if steer < 0:
direction = 3
elif steer > 0:
direction = 1
else:
direction = 2
if not enabled:
direction = 2
self.assertEqual(direction, parser.vl['STEERING_IPAS']['DIRECTION_CMD'])
expected_steer = int(round(steer / 1.5)) * 1.5 if enabled else 0
self.assertAlmostEqual(expected_steer, parser.vl['STEERING_IPAS']['ANGLE'])
sendcan.close()
def test_steering(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
limit = 1500
# Get parser
parser_signals = [
('STEER_REQUEST', 'STEERING_LKA', 0),
('SET_ME_1', 'STEERING_LKA', 0),
('STEER_TORQUE_CMD', 'STEERING_LKA', -1),
('LKA_STATE', 'STEERING_LKA', -1),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for steer in np.linspace(-1., 1., 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.steer = float(steer)
control.enabled = True
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
CI.CS.steer_torque_motor = limit * steer
# More control applies for the first one because of rate limits
for _ in range(1000 if steer < -0.99 else 25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
parser.update(int(sec_since_boot() * 1e9), False)
self.assertEqual(1, parser.vl['STEERING_LKA']['SET_ME_1'])
self.assertEqual(True, parser.vl['STEERING_LKA']['STEER_REQUEST'])
self.assertAlmostEqual(round(steer * limit), parser.vl['STEERING_LKA']['STEER_TORQUE_CMD'])
self.assertEqual(0, parser.vl['STEERING_LKA']['LKA_STATE'])
sendcan.close()
def test_accel(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('ACCEL_CMD', 'ACC_CONTROL', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for accel in np.linspace(-3., 1.5, 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
gas = accel / 3. if accel > 0. else 0.
brake = -accel / 3. if accel < 0. else 0.
actuators.gas = float(gas)
actuators.brake = float(brake)
control.enabled = True
control.actuators = actuators
CI.update(control)
# More control applies for the first one because of rate limits
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
min_accel = accel - 0.061
max_accel = accel + 0.061
sent_accel = parser.vl['ACC_CONTROL']['ACCEL_CMD']
accel_ok = min_accel <= sent_accel <= max_accel
self.assertTrue(accel_ok, msg="%.2f <= %.2f <= %.2f" % (min_accel, sent_accel, max_accel))
sendcan.close()
def test_fcw(self):
# TODO: This message has a 0xc1 setme which is not yet checked or in the dbc file
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('FCW', 'ACC_HUD', 0),
('SET_ME_X20', 'ACC_HUD', 0),
('SET_ME_X10', 'ACC_HUD', 0),
('SET_ME_X80', 'ACC_HUD', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for fcw in [True, False]:
control = car.CarControl.new_message()
control.enabled = True
hud = car.CarControl.HUDControl.new_message()
if fcw:
hud.visualAlert = VA.fcw
control.hudControl = hud
CI.update(control)
for _ in range(200):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(fcw, parser.vl['ACC_HUD']['FCW'])
self.assertEqual(0x20, parser.vl['ACC_HUD']['SET_ME_X20'])
self.assertEqual(0x10, parser.vl['ACC_HUD']['SET_ME_X10'])
self.assertEqual(0x80, parser.vl['ACC_HUD']['SET_ME_X80'])
def ui_thread(addr, frame_address):
context = zmq.Context()
# TODO: Detect car from replay and use that to select carparams
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
VM = VehicleModel(CP)
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]])
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
if HOR:
size = (640+384+640, 960)
write_x = 5
write_y = 680
else:
size = (640+384, 960+300)
write_x = 645
write_y = 970
pygame.display.set_caption("openpilot debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
alert1_font = pygame.font.SysFont("arial", 30)
alert2_font = pygame.font.SysFont("arial", 20)
info_font = pygame.font.SysFont("arial", 15)
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24)
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8)
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port))
frame.setsockopt(zmq.SUBSCRIBE, "")
carState = sub_sock(context, service_list['carState'].port, addr=addr, conflate=True)
plan = sub_sock(context, service_list['plan'].port, addr=addr, conflate=True)
carControl = sub_sock(context, service_list['carControl'].port, addr=addr, conflate=True)
radar_state_sock = sub_sock(context, service_list['radarState'].port, addr=addr, conflate=True)
liveCalibration = sub_sock(context, service_list['liveCalibration'].port, addr=addr, conflate=True)
controls_state_sock = sub_sock(context, service_list['controlsState'].port, addr=addr, conflate=True)
liveTracks = sub_sock(context, service_list['liveTracks'].port, addr=addr, conflate=True)
model = sub_sock(context, service_list['model'].port, addr=addr, conflate=True)
test_model = sub_sock(context, 8040, addr=addr, conflate=True)
liveMpc = sub_sock(context, service_list['liveMpc'].port, addr=addr, conflate=True)
liveParameters = sub_sock(context, service_list['liveParameters'].port, addr=addr, conflate=True)
v_ego, angle_steers, angle_steers_des, model_bias = 0., 0., 0., 0.
params_ao, params_ao_average, params_stiffness, params_sr = None, None, None, None
enabled = False
gas = 0.
accel_override = 0.
computer_gas = 0.
brake = 0.
steer_torque = 0.
curvature = 0.
computer_brake = 0.
plan_source = 'none'
long_control_state = 'none'
model_data = None
test_model_data = None
a_ego = 0.0
a_target = 0.0
d_rel, y_rel, lead_status = 0., 0., False
d_rel2, y_rel2, lead_status2 = 0., 0., False
v_ego, v_pid, v_cruise, v_override = 0., 0., 0., 0.
brake_lights = False
alert_text1, alert_text2 = "", ""
intrinsic_matrix = None
calibration = None
#img = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype='uint8')
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
good_lt = None
lid_overlay_blank = get_blank_lid_overlay(UP)
img_offset = (0, 0)
if vision_test:
visiontest = VisionTest(FULL_FRAME_SIZE, MODEL_INPUT_SIZE, None)
# plots
name_to_arr_idx = { "gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"steer_torque": 8,
"v_override": 9,
"v_cruise": 10,
"a_ego": 11,
"a_target": 12,
"accel_override": 13}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-5., 5.), (0., 75.), (-3.0, 2.0)]
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"],
["angle_steers", "angle_steers_des", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"]]
plot_colors = [["b", "b", "g", "r", "y"],
["b", "g", "r"],
["b", "g", "r", "y"],
["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"],
["-", "-", "-"],
["-", "-", "-", "-"],
["-", "-"]]
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True)
while 1:
list(pygame.event.get())
screen.fill((64,64,64))
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_surface, lid_overlay
# ***** frame *****
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3,3)
else:
# assume frame is flipped
yuv_transform = np.array([
[-1.0, 0.0, FULL_FRAME_SIZE[0]-1],
[ 0.0, -1.0, FULL_FRAME_SIZE[1]-1],
[ 0.0, 0.0, 1.0]
])
if yuv_img and len(yuv_img) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(FULL_FRAME_SIZE[1] * 3 // 2, -1)
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
cv2.warpAffine(imgff, np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2],
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = eon_intrinsics
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
if calibration is not None and yuv_img and vision_test:
model_input_yuv = visiontest.transform_contiguous(yuv_img,
np.dot(yuv_transform, calibration.model_to_full_frame).reshape(-1).tolist())
cv2.cvtColor(
np.frombuffer(model_input_yuv, dtype=np.uint8).reshape(MODEL_INPUT_SIZE[1] * 3 // 2, -1),
cv2.COLOR_YUV2RGB_I420,
dst=imgw)
else:
imgw.fill(0)
imgw_test_model = imgw.copy()
# ***** controlsState *****
controls_state = recv_one_or_none(controls_state_sock)
if controls_state is not None:
v_ego = controls_state.controlsState.vEgo
angle_steers = controls_state.controlsState.angleSteers
model_bias = controls_state.controlsState.angleModelBias
curvature = controls_state.controlsState.curvature
v_pid = controls_state.controlsState.vPid
enabled = controls_state.controlsState.enabled
alert_text1 = controls_state.controlsState.alertText1
alert_text2 = controls_state.controlsState.alertText2
long_control_state = controls_state.controlsState.longControlState
cs = recv_one_or_none(carState)
if cs is not None:
gas = cs.carState.gas
brake_lights = cs.carState.brakeLights
a_ego = cs.carState.aEgo
brake = cs.carState.brake
v_cruise = cs.carState.cruiseState.speed
cc = recv_one_or_none(carControl)
if cc is not None:
v_override = cc.carControl.cruiseControl.speedOverride
computer_brake = cc.carControl.actuators.brake
computer_gas = cc.carControl.actuators.gas
steer_torque = cc.carControl.actuators.steer * 5.
angle_steers_des = cc.carControl.actuators.steerAngle
accel_override = cc.carControl.cruiseControl.accelOverride
p = recv_one_or_none(plan)
if p is not None:
a_target = p.plan.aTarget
plan_source = p.plan.longitudinalPlanSource
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = angle_steers
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = angle_steers_des
plot_arr[-1, name_to_arr_idx['gas']] = gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = computer_gas
plot_arr[-1, name_to_arr_idx['user_brake']] = brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = steer_torque
plot_arr[-1, name_to_arr_idx['computer_brake']] = computer_brake
plot_arr[-1, name_to_arr_idx['v_ego']] = v_ego
plot_arr[-1, name_to_arr_idx['v_pid']] = v_pid
plot_arr[-1, name_to_arr_idx['v_override']] = v_override
plot_arr[-1, name_to_arr_idx['v_cruise']] = v_cruise
plot_arr[-1, name_to_arr_idx['a_ego']] = a_ego
plot_arr[-1, name_to_arr_idx['a_target']] = a_target
plot_arr[-1, name_to_arr_idx['accel_override']] = accel_override
# ***** model ****
# live model
md = recv_one_or_none(model)
if md:
model_data = extract_model_data(md)
if model_data:
plot_model(model_data, VM, v_ego, curvature, imgw, calibration,
top_down)
if test_model is not None:
test_md = recv_one_or_none(test_model)
if test_md:
test_model_data = extract_model_data(test_md)
if test_model_data:
plot_model(test_model_data, VM, v_ego, curvature, imgw_test_model,
calibration, top_down, 215)
# MPC
mpc = recv_one_or_none(liveMpc)
if mpc:
draw_mpc(mpc, top_down)
# LiveParams
params = recv_one_or_none(liveParameters)
if params:
params_ao = params.liveParameters.angleOffset
params_ao_average = params.liveParameters.angleOffsetAverage
params_stiffness = params.liveParameters.stiffnessFactor
params_sr = params.liveParameters.steerRatio
# **** tracks *****
# draw all radar points
lt = recv_one_or_none(liveTracks)
if lt is not None:
good_lt = lt
if good_lt is not None:
maybe_update_radar_points(good_lt, top_down[1])
# ***** radarState *****
radar_state = recv_one_or_none(radar_state_sock)
if radar_state is not None:
d_rel = radar_state.radarState.leadOne.dRel + RDR_TO_LDR
y_rel = radar_state.radarState.leadOne.yRel
lead_status = radar_state.radarState.leadOne.status
d_rel2 = radar_state.radarState.leadTwo.dRel + RDR_TO_LDR
y_rel2 = radar_state.radarState.leadTwo.yRel
lead_status2 = radar_state.radarState.leadTwo.status
lcal = recv_one_or_none(liveCalibration)
if lcal is not None:
calibration_message = lcal.liveCalibration
extrinsic_matrix = np.asarray(calibration_message.extrinsicMatrix).reshape(3, 4)
warp_matrix = np.asarray(calibration_message.warpMatrix2).reshape(3, 3)
calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix)
# draw red pt for lead car in the main img
if lead_status:
if calibration is not None:
dx, dy = draw_lead_on(img, d_rel, y_rel, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel, top_down)
# draw red pt for lead car2 in the main img
if lead_status2:
if calibration is not None:
dx2, dy2 = draw_lead_on(img, d_rel2, y_rel2, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel2, top_down)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
screen.blit(camera_surface, (0, 0))
# display alerts
alert_line1 = alert1_font.render(alert_text1, True, (255,0,0))
alert_line2 = alert2_font.render(alert_text2, True, (255,0,0))
screen.blit(alert_line1, (180, 150))
screen.blit(alert_line2, (180, 190))
if calibration is not None and img is not None:
cpw = warp_points(CalP, calibration.model_to_bb)
vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb)
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
pygame.draw.circle(screen, BLUE, map(int, map(round, vanishing_pointw[0])), 2)
if HOR:
screen.blit(draw_plots(plot_arr), (640+384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
screen.blit(cameraw_surface, (320, 480))
pygame.surfarray.blit_array(cameraw_test_surface, imgw_test_model.swapaxes(0, 1))
screen.blit(cameraw_test_surface, (0, 480))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640,0))
i = 0
SPACING = 25
# enabled
enabled_line = info_font.render("ENABLED", True, GREEN if enabled else BLACK)
screen.blit(enabled_line, (write_x, write_y + i * SPACING))
i += 1
# brake lights
brake_lights_line = info_font.render("BRAKE LIGHTS", True, RED if brake_lights else BLACK)
screen.blit(brake_lights_line, (write_x, write_y + i * SPACING))
i += 1
# speed
v_ego_line = info_font.render("SPEED: " + str(round(v_ego, 1)) + " m/s", True, YELLOW)
screen.blit(v_ego_line, (write_x, write_y + i * SPACING))
i += 1
# angle offset
model_bias_line = info_font.render("MODEL BIAS: " + str(round(model_bias, 2)) + " deg", True, YELLOW)
screen.blit(model_bias_line, (write_x, write_y + i * SPACING))
i += 1
# long control state
long_control_state_line = info_font.render("LONG CONTROL STATE: " + str(long_control_state), True, YELLOW)
screen.blit(long_control_state_line, (write_x, write_y + i * SPACING))
i += 1
# long mpc source
plan_source_line = info_font.render("LONG MPC SOURCE: " + str(plan_source), True, YELLOW)
screen.blit(plan_source_line, (write_x, write_y + i * SPACING))
i += 1
if params_ao is not None:
i += 1
angle_offset_avg_line = info_font.render("ANGLE OFFSET (AVG): " + str(round(params_ao_average, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_avg_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("ANGLE OFFSET (INSTANT): " + str(round(params_ao, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("STIFFNESS: " + str(round(params_stiffness * 100., 2)) + " %", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
steer_ratio_line = info_font.render("STEER RATIO: " + str(round(params_sr, 2)), True, YELLOW)
screen.blit(steer_ratio_line, (write_x, write_y + i * SPACING))
i += 1
# this takes time...vsync or something
pygame.display.flip()
def controlsd_thread(gctx=None, rate=100):
gc.disable()
# start the loop
set_realtime_priority(3)
##### AS
context = zmq.Context()
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
carla_socket = context.socket(zmq.PAIR)
carla_socket.bind("tcp://*:5560")
is_metric = True
passive = True
##### AS
# No sendcan if passive
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
# Sub sockets
poller = zmq.Poller()
#thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
#health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context,
service_list['liveCalibration'].port,
conflate=True,
poller=poller)
#driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
plan_sock = messaging.sub_sock(context,
service_list['plan'].port,
conflate=True,
poller=poller)
path_plan_sock = messaging.sub_sock(context,
service_list['pathPlan'].port,
conflate=True,
poller=poller)
#logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
CP.steerRatio = 1.0
CI = ToyotaInterface(CP, sendcan)
if CI is None:
raise Exception("unsupported car")
# if stock camera is connected, then force passive behavior
if not CP.enableCamera:
passive = True
sendcan = None
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(CP)
AM = AlertManager()
if not passive:
AM.add("startup", False)
state = State.enabled
soft_disable_timer = 0
v_cruise_kph = 50 ##### !!! change
v_cruise_kph_last = 0 ##### !! change
cal_status = Calibration.CALIBRATED
cal_perc = 0
plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
angle_offset = 0.
prof = Profiler(False) # off by default
startup = True ##### AS
while True:
start_time = int(sec_since_boot() * 1e9)
prof.checkpoint("Ratekeeper", ignore=True)
if cal_status != Calibration.CALIBRATED:
assert (1 == 0), 'Got uncalibrated for some reason'
stuff = recv_array(carla_socket)
current_vel = float(stuff[0])
current_steer = float(stuff[1])
v_cruise_kph = float(stuff[2])
updateInternalCS(CI.CS, current_vel, current_steer, 0, v_cruise_kph)
CS = returnNewCS(CI)
events = list(CS.events)
##### AS since we dont do preenabled state
if startup:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
v_acc = 0
a_acc = 0
AM.process_alerts(0.0)
startup = False
ASsend_live100_CS(plan, path_plan, CS, VM, state, events, v_cruise_kph,
AM, LaC, LoC, angle_offset, v_acc, a_acc, rk,
start_time, live100, carstate)
cal_status, cal_perc, plan, path_plan =\
ASdata_sample(plan_sock, path_plan_sock, cal, poller, cal_status, cal_perc, state, plan, path_plan)
prof.checkpoint("Sample")
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
plan_age = (start_time - plan.logMonoTime) / 1e9
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
print 'planner time too long or invalid'
#events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
#if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
# events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not passive:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
v_acc, a_acc = \
ASstate_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk,
LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
yawrate = VM.yaw_rate(math.radians(path_plan.pathPlan.angleSteers),
v_acc)
beta = (VM.aR + VM.aF * VM.chi -
VM.m * v_acc**2 / VM.cF / VM.cR / VM.l *
(VM.cF * VM.aF - VM.cR * VM.aR * VM.chi))
beta *= yawrate / (1 - VM.chi) / v_acc
send_array(
carla_socket,
np.array([
v_acc, path_plan.pathPlan.angleSteers,
-3.3 * math.degrees(yawrate), -3.3 * math.degrees(beta)
]))
prof.checkpoint("Sent")
rk.monitor_time() # Run at 100Hz, no 20 Hz
prof.display()
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
if CS.CP.enableGasInterceptor and enabled:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in [CAR.COROLLA]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pid_accel_limits = CarInterface.get_pid_accel_limits(self.CP, CS.out.vEgo, None) # Need to get cruise speed from somewhere
pcm_accel_cmd = clip(actuators.accel, pid_accel_limits[0], pid_accel_limits[1])
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def radard_thread(gctx=None):
set_realtime_priority(2)
##### AS
# # wait for stats about the car to come in from controls
# cloudlog.info("radard is waiting for CarParams")
# CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
# mocked = CP.carName == "mock"
# VM = VehicleModel(CP)
# cloudlog.info("radard got CarParams")
# # import the radar from the fingerprint
# cloudlog.info("radard is importing %s", CP.carName)
# RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
# context = zmq.Context()
cloudlog.info("radard is waiting for CarParams")
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
CP.steerRatio = 1.0
mocked = True
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is NOT importing %s but using mock radar instead (AS)", CP.carName)
RadarInterface = importlib.import_module('selfdrive.car.mock.radar_interface').RadarInterface
context = zmq.Context()
##### AS
# *** subscribe to features and model from visiond
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
MP = ModelParser()
RI = RadarInterface(CP)
last_md_ts = 0
last_l100_ts = 0
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 20. # model and radar are both at 20Hz
tsv = 1./rate
v_len = 20 # how many speed data points to remember for t alignment with rdr data
active = 0
steer_angle = 0.
steer_override = False
tracks = defaultdict(dict)
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
v_ego = None
v_ego_array = np.zeros([2, v_len])
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
v_ego_array = v_ego_array[:, 1:]
last_l100_ts = l100.logMonoTime
if v_ego is None:
continue
if md is not None:
last_md_ts = md.logMonoTime
# *** get path prediction from the model ***
MP.update(v_ego, md)
# run kalman filter only if prob is high enough
if MP.lead_prob > 0.7:
reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
ekfv.update_scalar(reading)
ekfv.predict(tsv)
# When changing lanes the distance to the lead car can suddenly change,
# which makes the Kalman filter output large relative acceleration
if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(MP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
if ids not in ar_pts:
tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not mocked:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
if DEBUG:
print("NEW CYCLE")
if VISION_POINT in ar_pts:
print("vision", ar_pts[VISION_POINT])
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(tracks[idens[0]])
else:
clusters = []
if DEBUG:
for i in clusters:
print(i)
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(v_ego)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
if lead_len > 0:
dat.live20.leadOne = lead_clusters[0].toLive20()
if lead2_len > 0:
dat.live20.leadTwo = lead2_clusters[0].toLive20()
else:
dat.live20.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary,
tracks[ids].measured))
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
"aRel": float(tracks[ids].aRel),
"stationary": bool(tracks[ids].stationary),
"oncoming": bool(tracks[ids].oncoming),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def ui_thread(addr, frame_address):
# TODO: Detect car from replay and use that to select carparams
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017")
VM = VehicleModel(CP)
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0],
[MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]],
[0, MODEL_INPUT_SIZE[1]]])
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
if HOR:
size = (640 + 384 + 640, 960)
write_x = 5
write_y = 680
else:
size = (640 + 384, 960 + 300)
write_x = 645
write_y = 970
pygame.display.set_caption("openpilot debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
alert1_font = pygame.font.SysFont("arial", 30)
alert2_font = pygame.font.SysFont("arial", 20)
info_font = pygame.font.SysFont("arial", 15)
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
frame = messaging.sub_sock('frame', addr=addr, conflate=True)
sm = messaging.SubMaster([
'carState', 'plan', 'carControl', 'radarState', 'liveCalibration',
'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters',
'pathPlan'
],
addr=addr)
calibration = None
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3),
dtype=np.uint8)
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
lid_overlay_blank = get_blank_lid_overlay(UP)
# plots
name_to_arr_idx = {
"gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"angle_steers_k": 8,
"steer_torque": 9,
"v_override": 10,
"v_cruise": 11,
"a_ego": 12,
"a_target": 13,
"accel_override": 14
}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]),
(0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.),
(-3.0, 2.0)]
plot_names = [[
"gas", "computer_gas", "user_brake", "computer_brake", "accel_override"
], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"]]
plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"],
["b", "g", "r", "y"], ["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"],
["-", "-", "-", "-"], ["-", "-"]]
draw_plots = init_plots(plot_arr,
name_to_arr_idx,
plot_xlims,
plot_ylims,
plot_names,
plot_colors,
plot_styles,
bigplots=True)
while 1:
list(pygame.event.get())
screen.fill((64, 64, 64))
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_surface, lid_overlay
# ***** frame *****
fpkt = messaging.recv_one(frame)
rgb_img_raw = fpkt.frame.image
if fpkt.frame.transform:
img_transform = np.array(fpkt.frame.transform).reshape(3, 3)
else:
# assume frame is flipped
img_transform = np.array([[-1.0, 0.0, FULL_FRAME_SIZE[0] - 1],
[0.0, -1.0, FULL_FRAME_SIZE[1] - 1],
[0.0, 0.0, 1.0]])
if rgb_img_raw and len(
rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3:
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape(
(FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
cv2.warpAffine(imgff,
np.dot(img_transform, _BB_TO_FULL_FRAME)[:2],
(img.shape[1], img.shape[0]),
dst=img,
flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = eon_intrinsics
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
if calibration is not None:
transform = np.dot(img_transform, calibration.model_to_full_frame)
imgw = cv2.warpAffine(imgff,
transform[:2],
(MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]),
flags=cv2.WARP_INVERSE_MAP)
else:
imgw.fill(0)
sm.update()
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm[
'controlsState'].lateralControlState.lqrState.steerAngle
elif w == 'indiState':
angle_steers_k = sm[
'controlsState'].lateralControlState.indiState.steerAngle
else:
angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:]
plot_arr[
-1,
name_to_arr_idx['angle_steers']] = sm['controlsState'].angleSteers
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm[
'carControl'].actuators.steerAngle
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
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm[
'carControl'].actuators.steer * ANGLE_SCALE
plot_arr[-1, name_to_arr_idx['computer_brake']] = sm[
'carControl'].actuators.brake
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['controlsState'].vEgo
plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid
plot_arr[-1, name_to_arr_idx['v_override']] = sm[
'carControl'].cruiseControl.speedOverride
plot_arr[
-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
plot_arr[-1, name_to_arr_idx['a_target']] = sm['plan'].aTarget
plot_arr[-1, name_to_arr_idx['accel_override']] = sm[
'carControl'].cruiseControl.accelOverride
# ***** model ****
if len(sm['model'].path.poly) > 0:
model_data = extract_model_data(sm['model'])
plot_model(model_data, VM, sm['controlsState'].vEgo,
sm['controlsState'].curvature, imgw, calibration,
top_down, np.array(sm['pathPlan'].dPoly))
# MPC
if sm.updated['liveMpc']:
draw_mpc(sm['liveMpc'], top_down)
# draw all radar points
maybe_update_radar_points(sm['liveTracks'], top_down[1])
if sm.updated['liveCalibration']:
extrinsic_matrix = np.asarray(
sm['liveCalibration'].extrinsicMatrix).reshape(3, 4)
ke = intrinsic_matrix.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke)
calibration = CalibrationTransformsForWarpMatrix(
warp_matrix, intrinsic_matrix, extrinsic_matrix)
# draw red pt for lead car in the main img
for lead in [sm['radarState'].leadOne, sm['radarState'].leadTwo]:
if lead.status:
if calibration is not None:
draw_lead_on(img,
lead.dRel,
lead.yRel,
calibration,
color=(192, 0, 0))
draw_lead_car(lead.dRel, top_down)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))
screen.blit(camera_surface, (0, 0))
# display alerts
alert_line1 = alert1_font.render(sm['controlsState'].alertText1, True,
(255, 0, 0))
alert_line2 = alert2_font.render(sm['controlsState'].alertText2, True,
(255, 0, 0))
screen.blit(alert_line1, (180, 150))
screen.blit(alert_line2, (180, 190))
if calibration is not None and img is not None:
cpw = warp_points(CalP, calibration.model_to_bb)
vanishing_pointw = warp_points(vanishing_point,
calibration.model_to_bb)
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
pygame.draw.circle(screen, BLUE,
list(map(int, map(round, vanishing_pointw[0]))),
2)
if HOR:
screen.blit(draw_plots(plot_arr), (640 + 384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
screen.blit(cameraw_surface, (320, 480))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640, 0))
i = 0
SPACING = 25
lines = [
info_font.render("ENABLED", True,
GREEN if sm['controlsState'].enabled else BLACK),
info_font.render("BRAKE LIGHTS", True,
RED if sm['carState'].brakeLights else BLACK),
info_font.render(
"SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True,
YELLOW),
info_font.render(
"LONG CONTROL STATE: " +
str(sm['controlsState'].longControlState), True, YELLOW),
info_font.render(
"LONG MPC SOURCE: " + str(sm['plan'].longitudinalPlanSource),
True, YELLOW), None,
info_font.render(
"ANGLE OFFSET (AVG): " +
str(round(sm['liveParameters'].angleOffsetAverage, 2)) +
" deg", True, YELLOW),
info_font.render(
"ANGLE OFFSET (INSTANT): " +
str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True,
YELLOW),
info_font.render(
"STIFFNESS: " +
str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) +
" %", True, YELLOW),
info_font.render(
"STEER RATIO: " +
str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)
]
for i, line in enumerate(lines):
if line is not None:
screen.blit(line, (write_x, write_y + i * SPACING))
# this takes time...vsync or something
pygame.display.flip()
def controlsd_thread(gctx=None, rate=100):
gc.disable()
# start the loop
set_realtime_priority(3)
##### AS
context = zmq.Context()
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
carla_socket = context.socket(zmq.PAIR)
carla_socket.bind("tcp://*:5560")
is_metric = True
passive = True
##### AS
# No sendcan if passive
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
# Sub sockets
poller = zmq.Poller()
#thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
#health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context,
service_list['liveCalibration'].port,
conflate=True,
poller=poller)
#driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
plan_sock = messaging.sub_sock(context,
service_list['plan'].port,
conflate=True,
poller=poller)
path_plan_sock = messaging.sub_sock(context,
service_list['pathPlan'].port,
conflate=True,
poller=poller)
#logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
CP.steerRatio = 1.0
CI = ToyotaInterface(CP, sendcan)
if CI is None:
raise Exception("unsupported car")
# if stock camera is connected, then force passive behavior
if not CP.enableCamera:
passive = True
sendcan = None
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(CP)
AM = AlertManager()
if not passive:
AM.add("startup", False)
state = State.enabled
soft_disable_timer = 0
v_cruise_kph = 50 ##### !!! change
v_cruise_kph_last = 0 ##### !! change
cal_status = Calibration.INVALID
cal_perc = 0
plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
angle_offset = 0.
prof = Profiler(False) # off by default
startup = True ##### AS
while True:
start_time = int(sec_since_boot() * 1e9)
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, plan, path_plan =\
data_sample(CI, CC, plan_sock, path_plan_sock, cal, poller, cal_status, cal_perc, state, plan, path_plan, v_cruise_kph)
prof.checkpoint("Sample")
##### AS since we dont do preenabled state
if startup:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
if cal_status != Calibration.CALIBRATED:
continue
startup = False
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
plan_age = (start_time - plan.logMonoTime) / 1e9
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
print 'planner time too long'
#events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
#if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
# events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not passive:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, angle_offset, v_acc, a_acc = \
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk,
LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, AM, LaC, LoC, angle_offset, passive,
start_time, v_acc, a_acc, carla_socket)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz, no 20 Hz
prof.display()
def generate_code():
dim_state = CarKalman.x_initial.shape[0]
name = CarKalman.name
maha_test_kinds = CarKalman.maha_test_kinds
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
# TODO: Read from car params at runtime
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
VM = VehicleModel(CP)
m = VM.m
j = VM.j
aF = VM.aF
aR = VM.aR
x = state[States.STIFFNESS, :][0, 0]
cF, cR = x * VM.cF, x * VM.cR
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[
sp.Matrix([angle_offset_fast]),
ObservationKind.ANGLE_OFFSET_FAST, None
],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
]
gen_code(name,
f_sym,
dt,
state_sym,
obs_eqs,
dim_state,
dim_state,
maha_test_kinds=maha_test_kinds)
def plannerd_thread():
##### AS
# context = zmq.Context()
# params = Params()
# # Get FCW toggle from settings
# fcw_enabled = params.get("IsFcwEnabled") == "1"
# cloudlog.info("plannerd is waiting for CarParams")
# CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
# cloudlog.info("plannerd got CarParams: %s", CP.carName)
context = zmq.Context()
fcw_enabled = False
cloudlog.info("plannerd is waiting for CarParams")
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
CP.steerRatio = 1.0
cloudlog.info("plannerd got CarParams: %s", CP.carName)
##### AS
PL = Planner(CP, fcw_enabled)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
poller = zmq.Poller()
car_state_sock = messaging.sub_sock(context,
service_list['carState'].port,
conflate=True,
poller=poller)
live100_sock = messaging.sub_sock(context,
service_list['live100'].port,
conflate=True,
poller=poller)
live20_sock = messaging.sub_sock(context,
service_list['live20'].port,
conflate=True,
poller=poller)
model_sock = messaging.sub_sock(context,
service_list['model'].port,
conflate=True,
poller=poller)
live_map_data_sock = messaging.sub_sock(context,
service_list['liveMapData'].port,
conflate=True,
poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
live100 = messaging.new_message()
live100.init('live100')
model = messaging.new_message()
model.init('model')
live20 = messaging.new_message()
live20.init('live20')
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
startup = True
while True:
recv_live100 = False
recv_carstate = False
recv_model = False
recv_live20 = False
while not (recv_live100 and recv_carstate and recv_model
and recv_live20):
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
recv_live100 = True
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
recv_carstate = True
if startup:
reset_speed = car_state.carState.vEgo
reset_accel = min(car_state.carState.aEgo, 0.0)
PL.v_acc = reset_speed
PL.a_acc = reset_accel
PL.v_acc_start = reset_speed
PL.a_acc_start = reset_accel
PL.v_cruise = reset_speed
PL.a_cruise = reset_accel
startup = False
elif socket is model_sock:
model = messaging.recv_one(socket)
recv_model = True
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
live20 = messaging.recv_one(socket)
recv_live20 = True
if recv_model and recv_live100 and recv_carstate:
PP.update(CP, VM, car_state, model, live100)
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
else:
return kin_ss_sol(sa, u, self.CP)
def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions
return self.curvature_factor(u) * sa / self.CP.sR
def curvature_factor(self, u):
sf = calc_slip_factor(self.CP)
return (1. - self.CP.chi) / (1. - sf * u**2) / self.CP.l
def get_steer_from_curvature(self, curv, u):
return curv * self.CP.sR * 1.0 / self.curvature_factor(u)
def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self.CP)
return np.matmul(
(A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
if __name__ == '__main__':
from selfdrive.car.toyota.interface import CarInterface
# load car params
CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
print CP
VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15)
print calc_slip_factor(CP)
def test_ui(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('BARRIERS', 'LKAS_HUD', -1),
('RIGHT_LINE', 'LKAS_HUD', 0),
('LEFT_LINE', 'LKAS_HUD', 0),
('SET_ME_X01', 'LKAS_HUD', 0),
('SET_ME_X01_2', 'LKAS_HUD', 0),
('LDA_ALERT', 'LKAS_HUD', -1),
('SET_ME_X0C', 'LKAS_HUD', 0),
('SET_ME_X2C', 'LKAS_HUD', 0),
('SET_ME_X38', 'LKAS_HUD', 0),
('SET_ME_X02', 'LKAS_HUD', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for left_lane in [True, False]:
for right_lane in [True, False]:
for steer in [True, False]:
control = car.CarControl.new_message()
control.enabled = True
hud = car.CarControl.HUDControl.new_message()
if steer:
hud.visualAlert = VA.steerRequired
hud.leftLaneVisible = left_lane
hud.rightLaneVisible = right_lane
control.hudControl = hud
CI.update(control)
for _ in range(200): # UI is only sent at 1Hz
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x0c, parser.vl['LKAS_HUD']['SET_ME_X0C'])
self.assertEqual(0x2c, parser.vl['LKAS_HUD']['SET_ME_X2C'])
self.assertEqual(0x38, parser.vl['LKAS_HUD']['SET_ME_X38'])
self.assertEqual(0x02, parser.vl['LKAS_HUD']['SET_ME_X02'])
self.assertEqual(0, parser.vl['LKAS_HUD']['BARRIERS'])
self.assertEqual(1 if right_lane else 2, parser.vl['LKAS_HUD']['RIGHT_LINE'])
self.assertEqual(1 if left_lane else 2, parser.vl['LKAS_HUD']['LEFT_LINE'])
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01'])
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01_2'])
self.assertEqual(steer, parser.vl['LKAS_HUD']['LDA_ALERT'])
def test_standstill_and_cancel(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('RELEASE_STANDSTILL', 'ACC_CONTROL', 0),
('CANCEL_REQ', 'ACC_CONTROL', 0),
('SET_ME_X3', 'ACC_CONTROL', 0),
('SET_ME_1', 'ACC_CONTROL', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
control = car.CarControl.new_message()
control.enabled = True
CI.update(control)
CI.CS.pcm_acc_status = 8 # Active
CI.CS.standstill = True
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x3, parser.vl['ACC_CONTROL']['SET_ME_X3'])
self.assertEqual(1, parser.vl['ACC_CONTROL']['SET_ME_1'])
self.assertFalse(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
self.assertFalse(parser.vl['ACC_CONTROL']['CANCEL_REQ'])
CI.CS.pcm_acc_status = 7 # Standstill
for _ in range(10):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertTrue(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
cruise = car.CarControl.CruiseControl.new_message()
cruise.cancel = True
control.cruiseControl = cruise
for _ in range(10):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertTrue(parser.vl['ACC_CONTROL']['CANCEL_REQ'])
