def data_sample(CI, CC, can_sock, carstate, lac_log, lateral, sm, profiler):
"""Receive data from sockets and create events for battery, temperature and disk space"""
can_strs = [can_sock.recv()]
profiler.checkpoint('can_recv', True)
CS = CI.update(CC, can_strs, lac_log, profiler)
profiler.checkpoint('carstate')
lateral.update(CS, sm, 0, 1)
profiler.checkpoint('lateral')
if CS.canTime + 20 < CS.sysTime:
can_strs = messaging.drain_sock_raw(can_sock,
wait_for_one=False,
limit=1)
profiler.checkpoint('drain_can')
if len(can_strs) > 0:
print(
" Controls lagged by %d CAN packets at %d at %0.2f m/s!" %
(len(can_strs), int(time.time() * 1000), CS.vEgo),
[len(x) for x in can_strs])
for i in range(len(can_strs[-40:])):
#time.sleep(0.00001)
CS = CI.update(CC, [can_strs[i]], lac_log, profiler)
profiler.checkpoint('drain_carstate')
lateral.update(CS, sm, i, len(can_strs[-40:]))
profiler.checkpoint('drain_lateral')
else:
#print(" CAN lagged!")
CI.canTime += 20
#else:
# time.sleep(0.00001)
events = list(CS.events)
return CS, events
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 data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp,
free_space, low_battery, driver_status, state,
mismatch_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
enabled = isEnabled(state)
# Check for CAN timeout
if not can_strs:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if sm.updated['thermal']:
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm[
'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm[
'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# Handle calibration
if sm.updated['liveCalibration']:
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
if sm.updated['health']:
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], params)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
def steer_thread():
poller = zmq.Poller()
logcan = messaging.sub_sock(service_list['can'].port)
joystick_sock = messaging.sub_sock(service_list['testJoystick'].port,
conflate=True,
poller=poller)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
button_1_last = 0
enabled = False
CI, CP = get_car(logcan, sendcan)
CC = car.CarControl.new_message()
joystick = messaging.recv_one(joystick_sock)
while True:
# send
for socket, event in poller.poll(0):
if socket is joystick_sock:
joystick = messaging.recv_one(socket)
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
CS = CI.update(CC, can_strs)
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
actuators = car.CarControl.Actuators.new_message()
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
button_1 = joystick.testJoystick.buttons[1]
if button_1 and not button_1_last:
enabled = not enabled
button_1_last = button_1
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
hud_alert = 0
audible_alert = 0
if joystick.testJoystick.buttons[2]:
audible_alert = "beepSingle"
if joystick.testJoystick.buttons[3]:
audible_alert = "chimeRepeated"
hud_alert = "steerRequired"
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd
CC.enabled = enabled
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
# Limit to 100 frames per second
time.sleep(0.01)
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()
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 steer_thread():
poller = messaging.Poller()
logcan = messaging.sub_sock('can')
health = messaging.sub_sock('health')
joystick_sock = messaging.sub_sock('testJoystick',
conflate=True,
poller=poller)
carstate = messaging.pub_sock('carState')
carcontrol = messaging.pub_sock('carControl')
sendcan = messaging.pub_sock('sendcan')
button_1_last = 0
enabled = False
# wait for health and CAN packets
hw_type = messaging.recv_one(health).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(logcan)
CI, CP = get_car(logcan, sendcan, has_relay)
Params().put("CarParams", CP.to_bytes())
CC = car.CarControl.new_message()
while True:
# send
joystick = messaging.recv_one(joystick_sock)
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
CS = CI.update(CC, can_strs)
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
actuators = car.CarControl.Actuators.new_message()
if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
button_1 = joystick.testJoystick.buttons[1]
if button_1 and not button_1_last:
enabled = not enabled
button_1_last = button_1
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
hud_alert = 0
audible_alert = 0
if joystick.testJoystick.buttons[2]:
audible_alert = "beepSingle"
if joystick.testJoystick.buttons[3]:
audible_alert = "chimeRepeated"
hud_alert = "steerRequired"
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd
CC.enabled = enabled
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
MINY = -1.0
MAXY = 1.0
def get_rrext_by_trackId(rrext, trackId):
if rrext is not None:
for p in rrext:
if p.trackId == trackId:
return p
return None
if __name__ == "__main__":
CP = None
RI = RadarInterface(CP)
can_sock = messaging.sub_sock('can')
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr, rrext, ahb = RI.update(can_strings, 0.)
if (rr is None) or (rrext is None):
continue
print(chr(27) + "[2J")
for pt in rr.points:
if (pt.dRel >= MINX) and (pt.dRel <= MAXX) and (
pt.yRel >= MINY) and (pt.yRel <= MAXY):
extpt = get_rrext_by_trackId(rrext, pt.trackId)
print(pt, extpt)
