def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# 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"
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
if can_sock is None:
can_sock = messaging.sub_sock(service_list['can'].port)
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def plannerd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(2)
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)
PL = Planner(CP)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
if sm is None:
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'liveMapData'])
if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
sm['liveParameters'].valid = True
sm['liveParameters'].sensorValid = True
sm['liveParameters'].steerRatio = CP.steerRatio
sm['liveParameters'].stiffnessFactor = 1.0
while True:
sm.update()
if sm.updated['model']:
PP.update(sm, pm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, pm, CP, VM, PP)
def plannerd_thread():
gc.disable()
# start the loop
set_realtime_priority(2)
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)
PL = Planner(CP, fcw_enabled)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
sm = messaging.SubMaster(
['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
sm['liveParameters'].valid = True
sm['liveParameters'].sensorValid = True
sm['liveParameters'].steerRatio = CP.steerRatio
sm['liveParameters'].stiffnessFactor = 1.0
while True:
sm.update()
if sm.updated['model']:
PP.update(sm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, CP, VM, PP)
def calibrationd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
sm.update()
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
calibrator.send_data(pm)
def controlsd_thread(gctx=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub Sockets
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sm = messaging.SubMaster([
'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan',
'pathPlan'
])
logcan = messaging.sub_sock(service_list['can'].port)
CI, CP = get_car(logcan, sendcan)
logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
can_sock = messaging.sub_sock(service_list['can'].port, timeout=100)
CC = car.CarControl.new_message()
AM = AlertManager()
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
else:
LaC = LatControlINDI(CP)
driver_status = DriverStatus()
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
sm['pathPlan'].sensorValid = True
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'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 read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, 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, driver_status, v_acc, a_acc, lac_log = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc,
a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
'gpsLocation'], ignore_alive=['gpsLocation'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port,
timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
print("Waiting for CAN messages...")
get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available or CP.dashcamOnly
if read_only:
CP.safetyModel = CP.safetyModelPassive
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'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 read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, 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, driver_status, v_acc, a_acc, lac_log = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only,
start_time, v_acc, a_acc, lac_log,
events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# 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))
use_tesla_radar = CarSettings().get_value("useTeslaRadar")
mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar)
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
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'pathPlan'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
icLeads = messaging.pub_sock('uiIcLeads')
ahbInfo = messaging.pub_sock('ahbInfo')
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None)
RD = RadarD(mocked, RI, use_tesla_radar,RI.delay)
has_radar = not CP.radarOffCan or mocked
last_md_ts = 0.
v_ego = 0.
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
sm.update(0)
if sm.updated['controlsState']:
v_ego = sm['controlsState'].vEgo
rr,rrext,ahbCarDetected = RI.update(can_strings,v_ego)
if rr is None:
continue
dat,datext = RD.update(rk.frame, sm, rr, has_radar, rrext)
dat.radarState.cumLagMs = -rk.remaining*1000.
pm.send('radarState', dat)
icLeads.send(datext.to_bytes())
ahbInfoMsg = tesla.AHBinfo.new_message()
ahbInfoMsg.source = 0
ahbInfoMsg.radarCarDetected = ahbCarDetected
ahbInfoMsg.cameraCarDetected = False
ahbInfo.send(ahbInfoMsg.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def radard_thread(gctx=None):
set_realtime_priority(2)
# 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))
use_tesla_radar = CarSettings().get_value("useTeslaRadar")
mocked = (CP.carName == "mock") or ((CP.carName == "tesla")
and not use_tesla_radar)
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
can_sock = messaging.sub_sock(service_list['can'].port)
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
RI = RadarInterface(CP)
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
icLeads = messaging.pub_sock(service_list['uiIcLeads'].port)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked, RI)
has_radar = not CP.radarOffCan or mocked
last_md_ts = 0.
v_ego = 0.
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr, rrext = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
if sm.updated['controlsState']:
v_ego = sm['controlsState'].vEgo
dat, datext = RD.update(rk.frame, RI.delay, sm, rr, has_radar, rrext)
dat.radarState.cumLagMs = -rk.remaining * 1000.
radarState.send(dat.to_bytes())
icLeads.send(datext.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
from common.basedir import BASEDIR
from selfdrive import messaging
if __name__ == "__main__":
sound_dir = os.path.join(BASEDIR, "selfdrive/assets/sounds")
sound_files = [f for f in os.listdir(sound_dir) if f.endswith(".wav")]
play_sound = os.path.join(BASEDIR, "selfdrive/ui/test/play_sound")
print("disabling charging")
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
os.environ["LD_LIBRARY_PATH"] = ""
sm = messaging.SubMaster(["thermal"])
FNULL = open(os.devnull, "w")
start_time = time.time()
while True:
volume = 15
n = random.randint(5, 10)
procs = []
for _ in range(n):
sound = random.choice(sound_files)
p = subprocess.Popen(
[play_sound,
os.path.join(sound_dir, sound),
str(volume)],
stdout=FNULL,
def radard_thread(gctx=None):
set_realtime_priority(2)
# 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
can_sock = messaging.sub_sock(service_list['can'].port)
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
RI = RadarInterface(CP)
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(VM, mocked)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr)
dat.radarState.cumLagMs = -rk.remaining * 1000.
radarState.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
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 controlsd_thread(gctx=None):
setproctitle('controlsd')
params = Params()
print(params)
# Pub Sockets
profiler = Profiler(True, 'controls')
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = None #messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
sm = messaging.SubMaster(['pathPlan', 'health', 'gpsLocationExternal'])
can_sock = messaging.sub_sock(service_list['can'].port)
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
print("panda black: ", is_panda_black)
wait_for_can(can_sock)
CI, CP = get_car(can_sock, sendcan, is_panda_black)
#logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
#can_timeout = None #if os.environ.get('NO_CAN_TIMEOUT', False) else 100
#can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(True, True)
AM.add(sm.frame, startup_alert, False)
LaC = LatControlPID(CP)
lateral = Lateral(CP)
lkasMode = int(float(LaC.kegman.conf['lkasMode']))
#CI.CS.lkasMode = (lkasMode == 0)
lac_log = None #car.CarState.lateralControlState.pidState.new_message()
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
events_prev = []
frame = 0
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
while True:
start_time = 0 # time.time() #sec_since_boot()
# Sample data and compute car events
CS, events = data_sample(CI, CC, can_sock, carstate, lac_log, lateral,
sm, profiler)
profiler.checkpoint('data_sample')
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
profiler.checkpoint('state_transition')
# Compute actuators (runs PID loops and lateral MPC)
sm.update(0)
profiler.checkpoint('sm_update')
actuators, lac_log = state_control(sm.frame, lkasMode, sm['pathPlan'],
CS, CP, state, events, AM, LaC,
lac_log, profiler)
profiler.checkpoint('state_control')
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, state, events, actuators,
carstate, carcontrol, carevents, carparams,
controlsstate, sendcan, AM, LaC,
start_time, lac_log, events_prev, profiler)
profiler.checkpoint('data_send')
frame += 1
if frame % 10000 == 0 and profiler.enabled:
profiler.display()
profiler.reset(True)
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()
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 = messaging.sub_sock('frame', addr=addr, conflate=True)
sm = messaging.SubMaster([
'carState', 'plan', 'carControl', 'radarState', 'liveCalibration',
'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters',
'pathPlan'
])
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)
counter = 0
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 radard_thread(gctx=None):
set_realtime_priority(2)
# 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
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# Default parameters
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
MP = ModelParser()
RI = RadarInterface(CP)
last_md_ts = 0
last_controls_state_ts = 0
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 1. / DT_MDL # model and radar are both at 20Hz
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 = 0.
v_ego_hist_t = deque([0], maxlen=v_len)
v_ego_hist_v = deque([0], maxlen=v_len)
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=None)
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]
sm.update(0)
if sm.updated['liveParameters']:
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
if sm.updated['controlsState']:
active = sm['controlsState'].active
v_ego = sm['controlsState'].vEgo
steer_angle = sm['controlsState'].angleSteers
steer_override = sm['controlsState'].steerOverride
v_ego_hist_v.append(v_ego)
v_ego_hist_t.append(float(rk.frame)/rate)
last_controls_state_ts = sm.logMonoTime['controlsState']
if sm.updated['model']:
last_md_ts = sm.logMonoTime['model']
MP.update(v_ego, sm['model'])
# 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(DT_MDL)
# 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=live_parameters.liveParameters.angleOffsetAverage)[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_hist_t, v_ego_hist_v)
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 = list(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:
cluster_idxs = cluster_points_centroid(track_pts, 2.5)
clusters = [None] * (max(cluster_idxs) + 1)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]
if clusters[cluster_i] is 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 radarState ***
dat = messaging.new_message()
dat.init('radarState')
dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
dat.radarState.mdMonoTime = last_md_ts
dat.radarState.canMonoTimes = list(rr.canMonoTimes)
dat.radarState.radarErrors = list(rr.errors)
dat.radarState.controlsStateMonoTime = last_controls_state_ts
if lead_len > 0:
dat.radarState.leadOne = lead_clusters[0].toRadarState()
if lead2_len > 0:
dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
else:
dat.radarState.leadTwo.status = False
else:
dat.radarState.leadOne.status = False
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.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()
