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()