def gps_planner_plan():
context = zmq.Context()
live_location = messaging.sub_sock(context,
'liveLocation',
conflate=True,
addr=_REMOTE_ADDR)
gps_planner_points = messaging.sub_sock(context,
'gpsPlannerPoints',
conflate=True)
gps_planner_plan = messaging.pub_sock(context, 'gpsPlannerPlan')
points = messaging.recv_one(gps_planner_points).gpsPlannerPoints
target_speed = 100. * CV.MPH_TO_MS
target_accel = 0.
last_ecef = np.array([0., 0., 0.])
while True:
ll = messaging.recv_one(live_location)
ll = ll.liveLocation
p = messaging.recv_one_or_none(gps_planner_points)
if p is not None:
points = p.gpsPlannerPoints
target_speed = p.gpsPlannerPoints.speedLimit
target_accel = p.gpsPlannerPoints.accelTarget
cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
# TODO: make NED initialization much faster so we can run this every time step
if np.linalg.norm(last_ecef - cur_ecef) > 200.:
ned_converter = NED(ll.lat, ll.lon, ll.alt)
last_ecef = cur_ecef
cur_heading = np.radians(ll.heading)
if points.valid:
poly, x_lookahead = fit_poly(points, cur_ecef, cur_heading,
ned_converter)
else:
poly, x_lookahead = [0.0, 0.0, 0.0, 0.0], 0.
valid = points.valid
m = messaging.new_message('gpsPlannerPlan')
m.gpsPlannerPlan.valid = valid
m.gpsPlannerPlan.poly = poly
m.gpsPlannerPlan.trackName = points.trackName
r = []
for p in points.points:
point = log.ECEFPoint.new_message()
point.x, point.y, point.z = p.x, p.y, p.z
r.append(point)
m.gpsPlannerPlan.points = r
m.gpsPlannerPlan.speed = target_speed
m.gpsPlannerPlan.acceleration = target_accel
m.gpsPlannerPlan.xLookahead = x_lookahead
gps_planner_plan.send(m.to_bytes())
def test_recv_one_or_none(self):
sock = "carState"
pub_sock = messaging.pub_sock(sock)
sub_sock = messaging.sub_sock(sock, timeout=1000)
zmq_sleep()
# no msg in queue, socket shouldn't block
recvd = messaging.recv_one(sub_sock)
self.assertTrue(recvd is None)
# one msg in queue
msg = random_carstate()
pub_sock.send(msg.to_bytes())
recvd = messaging.recv_one(sub_sock)
self.assertIsInstance(recvd, capnp._DynamicStructReader)
assert_carstate(msg.carState, recvd.carState)
def ui_thread(addr, frame_address):
context = zmq.Context()
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
size = (640 * SCALE, 480 * SCALE)
pygame.display.set_caption("comma one debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
camera_surface = pygame.surface.Surface((640 * SCALE, 480 * SCALE), 0,
24).convert()
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, 'frame'))
frame.setsockopt(zmq.SUBSCRIBE, "")
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3),
dtype=np.uint8)
while 1:
list(pygame.event.get())
screen.fill((64, 64, 64))
# ***** 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)
else:
img.fill(0)
height, width = img.shape[:2]
img_resized = cv2.resize(img, (SCALE * width, SCALE * height),
interpolation=cv2.INTER_CUBIC)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
screen.blit(camera_surface, (0, 0))
# this takes time...vsync or something
pygame.display.flip()
def regen_model(msgs, pm, frame_reader, model_sock):
# Send some livecalibration messages to initalize visiond
for msg in msgs:
if msg.which() == 'liveCalibration':
pm.send('liveCalibration', msg.as_builder())
out_msgs = []
fidx = 0
for msg in tqdm(msgs):
w = msg.which()
if w == 'roadCameraState':
msg = msg.as_builder()
img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:, :, ::-1]
msg.roadCameraState.image = img.flatten().tobytes()
pm.send(w, msg)
model = recv_one(model_sock)
fidx += 1
out_msgs.append(model)
elif w == 'liveCalibration':
pm.send(w, msg.as_builder())
return out_msgs
def process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs,
last_desire):
if msg.which() == "roadCameraState" and last_desire is not None:
dat = messaging.new_message('lateralPlan')
dat.lateralPlan.desire = last_desire
pm.send('lateralPlan', dat)
f = msg.as_builder()
pm.send(msg.which(), f)
img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0]
if msg.which == "roadCameraState":
vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK,
img.flatten().tobytes(), f.roadCameraState.frameId,
f.roadCameraState.timestampSof,
f.roadCameraState.timestampEof)
else:
vipc_server.send(VisionStreamType.VISION_STREAM_YUV_FRONT,
img.flatten().tobytes(), f.driverCameraState.frameId,
f.driverCameraState.timestampSof,
f.driverCameraState.timestampEof)
with Timeout(seconds=15):
log_msgs.append(
messaging.recv_one(sm.sock[packet_from_camera[msg.which()]]))
frame_idxs[msg.which()] += 1
if frame_idxs[msg.which()] >= frs[msg.which()].frame_count:
return None
update_spinner(spinner, frame_idxs['roadCameraState'],
frs['roadCameraState'].frame_count,
frame_idxs['driverCameraState'],
frs['driverCameraState'].frame_count)
return 0
def status_monitor():
# use driverState socker to drive timing.
driverState = messaging.sub_sock('driverState', addr=args.addr, conflate=True)
sm = messaging.SubMaster(['carState', 'dMonitoringState'], addr=args.addr)
steering_status = SteeringStatus()
while messaging.recv_one(driverState):
try:
sm.update()
# Get status and steering pressed value from our own instance of SteeringStatus
steering_status.update(Events(), sm['carState'].steeringPressed, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
steering_value = int(math.floor(steering_status.current_steering_pressed() * 9.99))
plot_list = ['-' for i in range(10)]
plot_list[steering_value] = '+'
# Get events from `dMonitoringState`
events = sm['dMonitoringState'].events
event_name = events[0].name if len(events) else "None"
# Print output
sys.stdout.write(f'\rsteering Pressed -> {"".join(plot_list)} | state: {steering_status.steering_state.name} | event: {event_name}')
sys.stdout.flush()
except Exception as e:
print(e)
def camera_replay(lr, fr, desire=None, calib=None):
spinner = Spinner()
spinner.update("starting model replay")
pm = messaging.PubMaster(['roadCameraState', 'liveCalibration', 'lateralPlan'])
sm = messaging.SubMaster(['modelV2'])
# TODO: add dmonitoringmodeld
print("preparing procs")
managed_processes['camerad'].prepare()
managed_processes['modeld'].prepare()
try:
print("starting procs")
managed_processes['camerad'].start()
managed_processes['modeld'].start()
time.sleep(5)
sm.update(1000)
print("procs started")
desires_by_index = {v:k for k,v in log.LateralPlan.Desire.schema.enumerants.items()}
cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]:
pm.send(msg.which(), replace_calib(msg, calib))
log_msgs = []
frame_idx = 0
for msg in tqdm(lr):
if msg.which() == "liveCalibration":
pm.send(msg.which(), replace_calib(msg, calib))
elif msg.which() == "roadCameraState":
if desire is not None:
for i in desire[frame_idx].nonzero()[0]:
dat = messaging.new_message('lateralPlan')
dat.lateralPlan.desire = desires_by_index[i]
pm.send('lateralPlan', dat)
f = msg.as_builder()
img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1]
f.roadCameraState.image = img.flatten().tobytes()
frame_idx += 1
pm.send(msg.which(), f)
with Timeout(seconds=15):
log_msgs.append(messaging.recv_one(sm.sock['modelV2']))
spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count))
if frame_idx >= fr.frame_count:
break
except KeyboardInterrupt:
pass
print("replay done")
spinner.close()
managed_processes['modeld'].stop()
time.sleep(2)
managed_processes['camerad'].stop()
return log_msgs
def cpp_replay_process(cfg, lr):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
sockets = {s: messaging.sub_sock(s, timeout=1000) for s in sub_sockets}
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
manager.prepare_managed_process(cfg.proc_name)
manager.start_managed_process(cfg.proc_name)
time.sleep(1) # We give the process time to start
log_msgs = []
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
for msg in tqdm(pub_msgs):
pm.send(msg.which(), msg.as_builder())
resp_sockets = sub_sockets if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is not None:
log_msgs.append(response)
manager.kill_managed_process(cfg.proc_name)
return log_msgs
def send_thread(sender_serial):
while True:
try:
if jungle:
sender = PandaJungle(sender_serial)
else:
sender = Panda(sender_serial)
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
sender.set_can_loopback(False)
can_sock = messaging.sub_sock('can')
while True:
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = list(filter(lambda x: x[-1] <= 2, snd))
try:
sender.can_send_many(snd)
except usb1.USBErrorTimeout:
pass
# Drain panda message buffer
sender.can_recv()
except Exception:
traceback.print_exc()
time.sleep(1)
def cpp_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
log_msgs = []
# We need to fake SubMaster alive since we can't inject a fake clock
setup_env(simulation=True)
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
try:
with Timeout(TIMEOUT):
while not all(
pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
time.sleep(0)
# Make sure all subscribers are connected
sockets = {
s: messaging.sub_sock(s, timeout=2000)
for s in sub_sockets
}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
for i, msg in enumerate(tqdm(pub_msgs, disable=False)):
pm.send(msg.which(), msg.as_builder())
resp_sockets = cfg.pub_sub[msg.which(
)] if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is None:
print(f"Warning, no response received {i}")
else:
response = response.as_builder()
response.logMonoTime = msg.logMonoTime
response = response.as_reader()
log_msgs.append(response)
if not len(
resp_sockets
): # We only need to wait if we didn't already wait for a response
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
finally:
managed_processes[cfg.proc_name].signal(signal.SIGKILL)
managed_processes[cfg.proc_name].stop()
return log_msgs
def ui_thread(addr, frame_address):
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
size = (int(_FULL_FRAME_SIZE[0] * SCALE), int(_FULL_FRAME_SIZE[1] * SCALE))
print(size)
pygame.display.set_caption("comma one debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
camera_surface = pygame.surface.Surface((_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE), 0, 24).convert()
frame = messaging.sub_sock('frame', conflate=True)
img = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype='uint8')
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
while 1:
list(pygame.event.get())
screen.fill((64, 64, 64))
# ***** frame *****
fpkt = messaging.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)
else:
# actually RGB
img = np.frombuffer(yuv_img, dtype=np.uint8).reshape((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3))
img = img[:, :, ::-1] # Convert BGR to RGB
height, width = img.shape[:2]
img_resized = cv2.resize(
img, (int(SCALE * width), int(SCALE * height)), interpolation=cv2.INTER_CUBIC)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
screen.blit(camera_surface, (0, 0))
# this takes time...vsync or something
pygame.display.flip()
def recv(can, addr):
received = False
r = []
while not received:
c = messaging.recv_one(can)
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
return r
def getMessage(service=None, timeout=1000):
if service is None or service not in service_list:
raise Exception("invalid service")
socket = messaging.sub_sock(service, timeout=timeout)
ret = messaging.recv_one(socket)
if ret is None:
raise TimeoutError
return ret.to_dict()
def reboot():
thermal_sock = messaging.sub_sock("thermal", timeout=1000)
ret = messaging.recv_one(thermal_sock)
if ret is None or ret.thermal.started:
raise Exception("Reboot unavailable")
def do_reboot():
time.sleep(2)
android.reboot()
threading.Thread(target=do_reboot).start()
return {"success": 1}
def reboot():
sock = messaging.sub_sock("deviceState", timeout=1000)
ret = messaging.recv_one(sock)
if ret is None or ret.deviceState.started:
raise Exception("Reboot unavailable")
def do_reboot():
time.sleep(2)
HARDWARE.reboot()
threading.Thread(target=do_reboot).start()
return {"success": 1}
def camera_replay(lr, fr):
spinner = Spinner()
pm = messaging.PubMaster(['frame', 'liveCalibration'])
sm = messaging.SubMaster(['model'])
# TODO: add dmonitoringmodeld
print("preparing procs")
manager.prepare_managed_process("camerad")
manager.prepare_managed_process("modeld")
try:
print("starting procs")
manager.start_managed_process("camerad")
manager.start_managed_process("modeld")
time.sleep(5)
print("procs started")
cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]:
pm.send(msg.which(), msg.as_builder())
log_msgs = []
frame_idx = 0
for msg in tqdm(lr):
if msg.which() == "liveCalibrationd":
pm.send(msg.which(), msg.as_builder())
elif msg.which() == "frame":
f = msg.as_builder()
img = fr.get(frame_idx, pix_fmt="rgb24")[0][:, ::, -1]
f.frame.image = img.flatten().tobytes()
frame_idx += 1
pm.send(msg.which(), f)
with Timeout(seconds=15):
log_msgs.append(messaging.recv_one(sm.sock['model']))
spinner.update("modeld replay %d/%d" %
(frame_idx, fr.frame_count))
if frame_idx >= fr.frame_count:
break
except KeyboardInterrupt:
pass
print("replay done")
spinner.close()
manager.kill_managed_process('modeld')
time.sleep(2)
manager.kill_managed_process('camerad')
return log_msgs
def status_monitor():
# use driverState socker to drive timing.
driverState = messaging.sub_sock('driverState',
addr=args.addr,
conflate=True)
sm = messaging.SubMaster(['carState', 'dMonitoringState'], addr=args.addr)
steering_status = HandsOnWheelStatus()
v_cruise_last = 0
while messaging.recv_one(driverState):
try:
sm.update()
v_cruise = sm['carState'].cruiseState.speed
steering_wheel_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or sm['carState'].steeringPressed
v_cruise_last = v_cruise
# Get status from our own instance of SteeringStatus
steering_status.update(Events(), steering_wheel_engaged,
sm['carState'].cruiseState.enabled,
sm['carState'].vEgo)
steering_state = steering_status.hands_on_wheel_state
state_name = "Unknown "
if steering_state == HandsOnWheelState.none:
state_name = "Not Active "
elif steering_state == HandsOnWheelState.ok:
state_name = "Hands On Wheel "
elif steering_state == HandsOnWheelState.minor:
state_name = "Hands Off Wheel - Minor "
elif steering_state == HandsOnWheelState.warning:
state_name = "Hands Off Wheel - Warning "
elif steering_state == HandsOnWheelState.critical:
state_name = "Hands Off Wheel - Critical"
elif steering_state == HandsOnWheelState.terminal:
state_name = "Hands Off Wheel - Terminal"
# Get events from `dMonitoringState`
events = sm['dMonitoringState'].events
event_name = events[0].name if len(events) else "None"
event_name = "{:<30}".format(event_name[:30])
# Print output
sys.stdout.write(
f'\rSteering State: {state_name} | event: {event_name}')
sys.stdout.flush()
except Exception as e:
print(e)
def cpp_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
os.environ["SIMULATION"] = "1" # Disable submaster alive checks
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
time.sleep(0)
# Make sure all subscribers are connected
sockets = {s: messaging.sub_sock(s, timeout=2000) for s in sub_sockets}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
log_msgs = []
for i, msg in enumerate(tqdm(pub_msgs, disable=CI)):
pm.send(msg.which(), msg.as_builder())
resp_sockets = cfg.pub_sub[msg.which(
)] if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is None:
print(f"Warning, no response received {i}")
else:
log_msgs.append(response)
if not len(
resp_sockets
): # We only need to wait if we didn't already wait for a response
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
managed_processes[cfg.proc_name].signal(signal.SIGKILL)
managed_processes[cfg.proc_name].stop()
return log_msgs
def send_thread(sender_serial):
global jungle
try:
if jungle:
sender = PandaJungle(sender_serial)
else:
sender = Panda(sender_serial)
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
sender.set_can_loopback(False)
can_sock = messaging.sub_sock('can')
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = list(filter(lambda x: x[-1] <= 2, snd))
sender.can_send_many(snd)
# Drain panda message buffer
sender.can_recv()
except Exception:
traceback.print_exc()
def model_replay(lr, frs):
spinner = Spinner()
spinner.update("starting model replay")
vipc_server = VisionIpcServer("camerad")
vipc_server.create_buffers(
VisionStreamType.VISION_STREAM_ROAD, 40, False,
*(tici_f_frame_size if TICI else eon_f_frame_size))
vipc_server.create_buffers(
VisionStreamType.VISION_STREAM_DRIVER, 40, False,
*(tici_d_frame_size if TICI else eon_d_frame_size))
vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40,
False, *(tici_f_frame_size))
vipc_server.start_listener()
sm = messaging.SubMaster(['modelV2', 'driverState'])
pm = messaging.PubMaster([
'roadCameraState', 'wideRoadCameraState', 'driverCameraState',
'liveCalibration', 'lateralPlan'
])
try:
managed_processes['modeld'].start()
managed_processes['dmonitoringmodeld'].start()
time.sleep(5)
sm.update(1000)
log_msgs = []
last_desire = None
recv_cnt = defaultdict(int)
frame_idxs = defaultdict(int)
# init modeld with valid calibration
cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"]
for _ in range(5):
pm.send(cal_msgs[0].which(), cal_msgs[0].as_builder())
time.sleep(0.1)
msgs = defaultdict(list)
for msg in lr:
msgs[msg.which()].append(msg)
for cam_msgs in zip_longest(msgs['roadCameraState'],
msgs['wideRoadCameraState'],
msgs['driverCameraState']):
# need a pair of road/wide msgs
if TICI and None in (cam_msgs[0], cam_msgs[1]):
break
for msg in cam_msgs:
if msg is None:
continue
if SEND_EXTRA_INPUTS:
if msg.which() == "liveCalibration":
last_calib = list(msg.liveCalibration.rpyCalib)
pm.send(msg.which(), replace_calib(msg, last_calib))
elif msg.which() == "lateralPlan":
last_desire = msg.lateralPlan.desire
dat = messaging.new_message('lateralPlan')
dat.lateralPlan.desire = last_desire
pm.send('lateralPlan', dat)
if msg.which() in VIPC_STREAM:
msg = msg.as_builder()
camera_state = getattr(msg, msg.which())
img = frs[msg.which()].get(frame_idxs[msg.which()],
pix_fmt="nv12")[0]
frame_idxs[msg.which()] += 1
# send camera state and frame
camera_state.frameId = frame_idxs[msg.which()]
pm.send(msg.which(), msg)
vipc_server.send(VIPC_STREAM[msg.which()],
img.flatten().tobytes(),
camera_state.frameId,
camera_state.timestampSof,
camera_state.timestampEof)
recv = None
if msg.which() in ('roadCameraState',
'wideRoadCameraState'):
if not TICI or min(frame_idxs['roadCameraState'],
frame_idxs['wideRoadCameraState']
) > recv_cnt['modelV2']:
recv = "modelV2"
elif msg.which() == 'driverCameraState':
recv = "driverState"
# wait for a response
with Timeout(15, f"timed out waiting for {recv}"):
if recv:
recv_cnt[recv] += 1
log_msgs.append(messaging.recv_one(sm.sock[recv]))
spinner.update(
"replaying models: road %d/%d, driver %d/%d" %
(frame_idxs['roadCameraState'],
frs['roadCameraState'].frame_count,
frame_idxs['driverCameraState'],
frs['driverCameraState'].frame_count))
if any(frame_idxs[c] >= frs[c].frame_count
for c in frame_idxs.keys()):
break
finally:
spinner.close()
managed_processes['modeld'].stop()
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if hw_type == HwType.whitePanda:
self.events.add(EventName.whitePandaUnsupported, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
hw_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = hw_type in [
PandaType.blackPanda, PandaType.uno, PandaType.dos
]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
self.enable_lte_onroad = params.get_bool("EnableLteOnroad")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.angle_steers_des = 0.
self.road_limit_speed = 0
self.road_limit_left_dist = 0
self.v_cruise_kph_limit = 0
self.curve_speed_ms = 255.
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# if hw_type == PandaType.greyPanda:
# self.events.add(EventName.startupGreyPanda, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
#!/usr/bin/env python3
import cereal.messaging as messaging
if __name__ == "__main__":
modeld_sock = messaging.sub_sock("modelV2")
last_frame_id = None
start_t = None
frame_cnt = 0
dropped = 0
while True:
m = messaging.recv_one(modeld_sock)
if m is None:
continue
frame_id = m.modelV2.frameId
t = m.logMonoTime / 1e9
frame_cnt += 1
if start_t is None:
start_t = t
last_frame_id = frame_id
continue
d_frame = frame_id - last_frame_id
dropped += d_frame - 1
expected_num_frames = int((t - start_t) * 20)
frame_drop = 100 * (1 - (expected_num_frames / frame_cnt))
print(
def mapsd_thread():
global last_gps
gps_sock = messaging.sub_sock('gpsLocation', conflate=True)
gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True)
map_data_sock = messaging.pub_sock('liveMapData')
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
# Outlier rejection
new_curvature = np.percentile(curvature, 90, interpolation='lower')
k = 0.6
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
in_turn_indices = curvature > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(dists[in_turn_indices])
else:
dist_to_turn = 999
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Speed limit
max_speed = cur_way.max_speed()
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())
def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
self.trace_log = trace1.Loger("controlsd")
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
socks = [
'thermal', 'health', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
]
self.sm = messaging.SubMaster(socks,
ignore_alive=['dMonitoringState'])
#self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
# 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
print(" start_Controls messages...1")
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
print(" start_Controls messages...2")
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl",
"1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
print('self.CP.lateralTuning.which()={}'.format(
self.CP.lateralTuning.which()))
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.consecutive_can_error_count = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = []
self.current_alert_types = []
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if self.read_only and not passive:
self.events.add(EventName.carUnrecognized, static=True)
# if hw_type == HwType.whitePanda:
# self.events.add(EventName.whitePandaUnsupported, static=True)
uname = subprocess.check_output(["uname", "-v"],
encoding='utf8').strip()
if uname == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020":
self.events.add(EventName.neosUpdateRequired, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.hyundai_lkas = self.read_only #read_only
self.init_flag = True
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
carControl = messaging.sub_sock('carControl',
addr=args.addr,
conflate=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'],
addr=args.addr)
msg_cnt = 0
stats = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0})
cnt = 0
total_error = 0
while messaging.recv_one(carControl):
sm.update()
msg_cnt += 1
actual_speed = sm['carState'].vEgo
enabled = sm['controlsState'].enabled
steer_override = sm['controlsState'].steerOverride
# must be above 10 m/s, engaged and not overriding steering
if actual_speed > 10.0 and enabled and not steer_override:
cnt += 1
# wait 5 seconds after engage/override
if cnt >= 500:
# calculate error before rounding
actual_angle = sm['controlsState'].angleSteers
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, HwType.dos]
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
if joystick.testJoystick.buttons[3]:
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('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
def ui_thread(addr, frame_address):
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
disp_info = pygame.display.Info()
max_height = disp_info.current_h
hor_mode = os.getenv("HORIZONTAL") is not None
hor_mode = True if max_height < 960 + 300 else hor_mode
if hor_mode:
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()
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
frame = messaging.sub_sock('roadCameraState', addr=addr, conflate=True)
sm = messaging.SubMaster([
'carState', 'longitudinalPlan', 'carControl', 'radarState',
'liveCalibration', 'controlsState', 'liveTracks', 'modelV2', 'liveMpc',
'liveParameters', 'lateralPlan', 'roadCameraState'
],
addr=addr)
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = None
num_px = 0
calibration = None
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.roadCameraState.image
num_px = len(rgb_img_raw) // 3
if rgb_img_raw and num_px in _FULL_FRAME_SIZE.keys():
FULL_FRAME_SIZE = _FULL_FRAME_SIZE[num_px]
imgff_shape = (FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3)
if imgff is None or imgff.shape != imgff_shape:
imgff = np.zeros(imgff_shape, dtype=np.uint8)
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
zoom_matrix = _BB_TO_FULL_FRAME[num_px]
cv2.warpAffine(imgff,
zoom_matrix[:2], (img.shape[1], img.shape[0]),
dst=img,
flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = _INTRINSICS[num_px]
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
sm.update()
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm[
'controlsState'].lateralControlState.lqrState.steeringAngleDeg
elif w == 'indiState':
angle_steers_k = sm[
'controlsState'].lateralControlState.indiState.steeringAngleDeg
else:
angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:]
plot_arr[
-1,
name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm[
'carControl'].actuators.steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
plot_arr[
-1,
name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas
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['carState'].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['longitudinalPlan'].aTarget
plot_arr[-1, name_to_arr_idx['accel_override']] = sm[
'carControl'].cruiseControl.accelOverride
if sm.rcv_frame['modelV2']:
plot_model(sm['modelV2'], img, calibration, top_down)
if sm.rcv_frame['radarState']:
plot_lead(sm['radarState'], top_down)
# draw all radar points
maybe_update_radar_points(sm['liveTracks'], top_down[1])
if sm.updated['liveCalibration'] and num_px:
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix)
# *** 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 hor_mode:
screen.blit(draw_plots(plot_arr), (640 + 384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640, 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['longitudinalPlan'].longitudinalPlanSource), True,
YELLOW), None,
info_font.render(
"ANGLE OFFSET (AVG): " +
str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) +
" deg", True, YELLOW),
info_font.render(
"ANGLE OFFSET (INSTANT): " +
str(round(sm['liveParameters'].angleOffsetDeg, 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(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
passive = passive or internet_needed
# 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', 'dMonitoringState', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
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
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("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)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
saturated_count = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# 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_perc, mismatch_counter, can_error_counter = data_sample(
CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
params)
prof.checkpoint("Sample")
# Create alerts
if not sm.alive['plan'] and sm.alive[
'pathPlan']: # only plan not being received: radar not communicating
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif 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 and os.getenv("NOSENSOR") is None:
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.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]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(
create_event('internetConnectivityNeeded',
[ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(
create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
events.append(
create_event(
'relayMalfunction',
[ET.NO_ENTRY, ET.PERMANENT, 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, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)
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, LaC, LoC,
read_only, start_time, v_acc, a_acc,
lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def gps_planner_point_selection():
DECIMATION = 1
cloudlog.info("Starting gps_plannerd point selection")
rk = Ratekeeper(10.0, print_delay_threshold=np.inf)
context = zmq.Context()
live_location = messaging.sub_sock(context,
'liveLocation',
conflate=True,
addr=_REMOTE_ADDR)
car_state = messaging.sub_sock(context, 'carState', conflate=True)
gps_planner_points = messaging.pub_sock(context, 'gpsPlannerPoints')
ui_navigation_event = messaging.pub_sock(context, 'uiNavigationEvent')
# Load tracks and instructions from disk
basedir = os.environ['BASEDIR']
tracks = np.load(
os.path.join(basedir,
'selfdrive/controls/tracks/%s.npy' % LOOP)).item()
instructions = json.loads(
open(
os.path.join(
basedir, 'selfdrive/controls/tracks/instructions_%s.json' %
LOOP)).read())
# Put tracks into KD-trees
track_trees = {}
for name in tracks:
tracks[name] = tracks[name][::DECIMATION]
track_trees[name] = cKDTree(tracks[name][:, 0:3]) # xyz
cur_track = None
source_track = None
target_track = None
instruction = None
v_ego = 0.
state = INSTRUCTION_STATE.NONE
counter = 0
while True:
counter += 1
ll = messaging.recv_one(live_location)
ll = ll.liveLocation
cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
cs = messaging.recv_one_or_none(car_state)
if cs is not None:
v_ego = cs.carState.vEgo
cur_track, closest_track = update_current_track(
tracks, cur_track, cur_ecef, track_trees)
#print cur_track
instruction = update_instruction(instruction, instructions, cur_track,
source_track, state, cur_ecef, tracks)
source_track, target_track = get_tracks_from_instruction(
tracks, instruction, track_trees, cur_ecef)
state, cur_track = calc_instruction_state(state, cur_track,
closest_track, source_track,
target_track, instruction)
active_points = []
# Make list of points used by gpsPlannerPlan
if cur_track is not None:
active_points = get_spaced_points(tracks[cur_track['name']],
cur_track['idx'], cur_ecef,
v_ego)
cur_pos = log.ECEFPoint.new_message()
cur_pos.x, cur_pos.y, cur_pos.z = map(float, cur_ecef)
m = messaging.new_message('gpsPlannerPoints')
m.gpsPlannerPoints.curPos = cur_pos
m.gpsPlannerPoints.points = convert_ecef_to_capnp(active_points)
m.gpsPlannerPoints.valid = len(active_points) > 10
m.gpsPlannerPoints.trackName = "none" if cur_track is None else cur_track[
'name']
m.gpsPlannerPoints.speedLimit = 100. if cur_track is None else float(
cur_track['speed'])
m.gpsPlannerPoints.accelTarget = 0. if cur_track is None else float(
cur_track['accel'])
gps_planner_points.send(m.to_bytes())
m = messaging.new_message('uiNavigationEvent')
m.uiNavigationEvent.status = state
m.uiNavigationEvent.type = "none" if instruction is None else instruction[
'type']
m.uiNavigationEvent.distanceTo = 0. if instruction is None else float(
instruction['distance'])
endRoadPoint = log.ECEFPoint.new_message()
m.uiNavigationEvent.endRoadPoint = endRoadPoint
ui_navigation_event.send(m.to_bytes())
rk.keep_time()