def main(): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} for i in range(1, 33): nav_frame_buffer[0][i] = {} if not CarSettings().get_value("useTeslaGPS"): gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') ubloxGnss = messaging.pub_sock('ubloxGnss') dev = init_reader() while True: try: msg = dev.receive_message() except serial.serialutil.SerialException as e: print(e) dev.close() dev = init_reader() if msg is not None: handle_msg(dev, msg, nav_frame_buffer) else: while True: time.sleep(1.1)
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: Plant.radar = messaging.pub_sock('radarState') Plant.controls_state = messaging.pub_sock('controlsState') Plant.car_state = messaging.pub_sock('carState') Plant.plan = messaging.sub_sock('longitudinalPlan') Plant.messaging_initialized = True self.v_lead_prev = 0.0 self.distance = 0. self.speed = speed self.acceleration = 0.0 # lead car self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy self.only_lead2=only_lead2 self.only_radar=only_radar self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate time.sleep(1) self.sm = messaging.SubMaster(['longitudinalPlan']) from selfdrive.car.honda.values import CAR from selfdrive.car.honda.interface import CarInterface self.CP = CarInterface.get_params(CAR.CIVIC) self.planner = Planner(self.CP, init_v=self.speed)
def main() -> NoReturn: log_handler = get_file_handler() log_handler.setFormatter(SwagLogFileFormatter(None)) log_level = 20 # logging.INFO ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) sock.bind("ipc:///tmp/logmessage") # and we publish them log_message_sock = messaging.pub_sock('logMessage') error_log_message_sock = messaging.pub_sock('errorLogMessage') while True: dat = b''.join(sock.recv_multipart()) level = dat[0] record = dat[1:].decode("utf-8") if level >= log_level: log_handler.emit(record) # then we publish them msg = messaging.new_message() msg.logMessage = record log_message_sock.send(msg.to_bytes()) if level >= 40: # logging.ERROR msg = messaging.new_message() msg.errorLogMessage = record error_log_message_sock.send(msg.to_bytes())
def cycle_alerts(duration=200, is_metric=False): alerts = list(EVENTS.keys()) print(alerts) alerts = [ EventName.preDriverDistracted, EventName.promptDriverDistracted, EventName.driverDistracted ] CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") sm = messaging.SubMaster([ 'deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman' ]) controls_state = messaging.pub_sock('controlsState') deviceState = messaging.pub_sock('deviceState') idx, last_alert_millis = 0, 0 events = Events() AM = AlertManager() frame = 0 while 1: if frame % duration == 0: idx = (idx + 1) % len(alerts) events.clear() events.add(alerts[idx]) current_alert_types = [ ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING ] a = events.create_alerts(current_alert_types, [CP, sm, is_metric]) AM.add_many(frame, a) AM.process_alerts(frame) dat = messaging.new_message() dat.init('controlsState') dat.controlsState.alertText1 = AM.alert_text_1 dat.controlsState.alertText2 = AM.alert_text_2 dat.controlsState.alertSize = AM.alert_size dat.controlsState.alertStatus = AM.alert_status dat.controlsState.alertBlinkingRate = AM.alert_rate dat.controlsState.alertType = AM.alert_type dat.controlsState.alertSound = AM.audible_alert controls_state.send(dat.to_bytes()) dat = messaging.new_message() dat.init('deviceState') dat.deviceState.started = True deviceState.send(dat.to_bytes()) frame += 1 time.sleep(0.01)
def __init__(self, carstate): self.CS = carstate self.uiCustomAlert = messaging.pub_sock('uiCustomAlert') self.uiButtonInfo = messaging.pub_sock('uiButtonInfo') self.uiSetCar = messaging.pub_sock('uiSetCar') self.uiPlaySound = messaging.pub_sock('uiPlaySound') self.uiGyroInfo = messaging.pub_sock('uiGyroInfo') self.uiButtonStatus = messaging.sub_sock('uiButtonStatus', conflate=True) self.prev_cstm_message = "" self.prev_cstm_status = -1
def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0): self.rate = rate if not Plant.messaging_initialized: Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') Plant.model = messaging.pub_sock('model') Plant.front_frame = messaging.pub_sock('frontFrame') Plant.live_params = messaging.pub_sock('liveParameters') Plant.live_location_kalman = messaging.pub_sock( 'liveLocationKalman') Plant.health = messaging.pub_sock('health') Plant.thermal = messaging.pub_sock('thermal') Plant.dMonitoringState = messaging.pub_sock('dMonitoringState') Plant.cal = messaging.pub_sock('liveCalibration') Plant.controls_state = messaging.sub_sock('controlsState') Plant.plan = messaging.sub_sock('plan') Plant.messaging_initialized = True self.frame = 0 self.angle_steer = 0. self.gear_choice = 0 self.speed, self.speed_prev = 0., 0. self.esp_disabled = 0 self.main_on = 1 self.user_gas = 0 self.computer_brake, self.user_brake = 0, 0 self.brake_pressed = 0 self.angle_steer_rate = 0 self.distance, self.distance_prev = 0., 0. self.speed, self.speed_prev = speed, speed self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0 self.gear_shifter = 8 # D gear self.pedal_gas = 0 self.cruise_setting = 0 self.seatbelt, self.door_all_closed = True, True self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0 # v_cruise is reported from can, not the one used for controls self.lead_relevancy = lead_relevancy # lead car self.distance_lead, self.distance_lead_prev = distance_lead, distance_lead self.rk = Ratekeeper(rate, print_delay_threshold=100) self.ts = 1. / rate self.cp = get_car_can_parser() self.response_seen = False time.sleep(1) messaging.drain_sock(Plant.sendcan) messaging.drain_sock(Plant.controls_state)
def __init__(self, radar_ts, mocked, RI, use_tesla_radar, delay=0): self.current_time = 0 self.mocked = mocked self.RI = RI self.tracks = defaultdict(dict) self.kalman_params = KalmanParams(radar_ts) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 # v_ego self.v_ego = 0. self.v_ego_hist = deque([0], maxlen=delay + 1) self.ready = False self.icCarLR = None self.use_tesla_radar = use_tesla_radar if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE) and self.use_tesla_radar: self.icCarLR = messaging.pub_sock('uiIcCarLR') self.lane_width = 3.0 #only used for left and right lanes self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max self.dPoly = [0., 0., 0., 0.]
def main(): rcv = sub_sock('can') # port 8006 snd = pub_sock('sendcan') # port 8017 time.sleep(0.3) # wait to bind before send/recv for i in range(10): print("Loop %d" % i) at = random.randint(1024, 2000) st = get_test_string()[0:8] snd.send( can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes()) time.sleep(0.1) res = drain_sock(rcv, True) assert len(res) == 1 res = res[0].can assert len(res) == 2 msg0, msg1 = res assert msg0.dat == st assert msg1.dat == st assert msg0.address == at assert msg1.address == at assert msg0.src == 0x80 | BUS assert msg1.src == BUS print("Success")
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 boardd_mock_loop(): can_init() handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') logcan = messaging.sub_sock('can') sendcan = messaging.pub_sock('sendcan') while 1: tsc = messaging.drain_sock(logcan, wait_for_one=True) snds = map(lambda x: can_capnp_to_can_list(x.can), tsc) snd = [] for s in snds: snd += s snd = list(filter(lambda x: x[-1] <= 2, snd)) snd_0 = len(list(filter(lambda x: x[-1] == 0, snd))) snd_1 = len(list(filter(lambda x: x[-1] == 1, snd))) snd_2 = len(list(filter(lambda x: x[-1] == 2, snd))) can_send_many(snd) # recv @ 100hz can_msgs = can_recv() got_0 = len(list(filter(lambda x: x[-1] == 0 + 0x80, can_msgs))) got_1 = len(list(filter(lambda x: x[-1] == 1 + 0x80, can_msgs))) got_2 = len(list(filter(lambda x: x[-1] == 2 + 0x80, can_msgs))) print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" % (len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2)) m = can_list_to_can_capnp(can_msgs, msgtype='sendcan') sendcan.send(m.to_bytes())
def test_recv_one_retry(self): sock = "carState" sock_timeout = 0.1 pub_sock = messaging.pub_sock(sock) sub_sock = messaging.sub_sock(sock, timeout=sock_timeout * 1000) zmq_sleep() # this test doesn't work with ZMQ since multiprocessing interrupts it if "ZMQ" not in os.environ: # wait 15 socket timeouts and make sure it's still retrying p = multiprocessing.Process(target=messaging.recv_one_retry, args=(sub_sock, )) p.start() time.sleep(sock_timeout * 15) self.assertTrue(p.is_alive()) p.terminate() # wait 15 socket timeouts before sending msg = random_carstate() delayed_send(sock_timeout * 15, pub_sock, msg.to_bytes()) start_time = time.monotonic() recvd = messaging.recv_one_retry(sub_sock) self.assertGreaterEqual(time.monotonic() - start_time, sock_timeout * 15) self.assertIsInstance(recvd, capnp._DynamicStructReader) assert_carstate(msg.carState, recvd.carState)
def boardd_proxy_loop(rate=100, address="192.168.2.251"): rk = Ratekeeper(rate) can_init() # *** subscribes can logcan = messaging.sub_sock('can', addr=address) # *** publishes to can send sendcan = messaging.pub_sock('sendcan') # drain sendcan to delete any stale messages from previous runs messaging.drain_sock(sendcan) while 1: # recv @ 100hz can_msgs = can_recv() #for m in can_msgs: # print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) # publish to logger # TODO: refactor for speed if len(can_msgs) > 0: dat = can_list_to_can_capnp(can_msgs, "sendcan") sendcan.send(dat) # send can if we have a packet tsc = messaging.recv_sock(logcan) if tsc is not None: cl = can_capnp_to_can_list(tsc.can) #for m in cl: # print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) can_send_many(cl) rk.keep_time()
def test_honda_ui_pcm_speed(self): self.longMessage = True sendcan = messaging.pub_sock('sendcan') car_name = HONDA.CIVIC params = CarInterface.get_params(car_name) CI = CarInterface(params, CarController) # Get parser parser_signals = [ # 780 - 0x30c ('PCM_SPEED', 'ACC_HUD', 99), ] parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") time.sleep(0.2) # Slow joiner syndrome for pcm_speed in np.linspace(0, 100, 20): cc = car.CarControl.CruiseControl.new_message() cc.speedOverride = float(pcm_speed * CV.KPH_TO_MS) control = car.CarControl.new_message() control.enabled = True control.cruiseControl = cc CI.update(control) for _ in range(25): can_sends = CI.apply(control) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) for _ in range(5): parser.update(int(sec_since_boot() * 1e9), False) time.sleep(0.01) self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_SPEED'], round(pcm_speed, 2), msg="Car: %s, speed: %.2f" % (car_name, pcm_speed))
def main(): # setup logentries. we forward log messages to it le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False) le_level = 20 #logging.INFO ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) sock.bind("ipc:///tmp/logmessage") # and we publish them pub_sock = messaging.pub_sock('logMessage') while True: dat = b''.join(sock.recv_multipart()) dat = dat.decode('utf8') # print "RECV", repr(dat) levelnum = ord(dat[0]) dat = dat[1:] if levelnum >= le_level: # push to logentries # TODO: push to athena instead le_handler.emit_raw(dat) # then we publish them msg = messaging.new_message() msg.logMessage = dat pub_sock.send(msg.to_bytes())
def cycle_alerts(duration_millis, alerts=None): if alerts is None: alerts = default_alerts controls_state = messaging.pub_sock('controlsState') last_pop_millis = now_millis() alert = alerts.pop() while 1: if (now_millis() - last_pop_millis) > duration_millis: alerts.insert(0, alert) alert = alerts.pop() last_pop_millis = now_millis() print('sending {}'.format(str(alert))) dat = messaging.new_message() dat.init('controlsState') dat.controlsState.alertType = alert.alert_type dat.controlsState.alertText1 = alert.alert_text_1 dat.controlsState.alertText2 = alert.alert_text_2 dat.controlsState.alertSize = alert.alert_size dat.controlsState.alertStatus = alert.alert_status dat.controlsState.alertSound = alert.audible_alert controls_state.send(dat.to_bytes()) time.sleep(0.01)
def main(): le_handler = get_le_handler() le_level = 20 # logging.INFO ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) sock.bind("ipc:///tmp/logmessage") # and we publish them pub_sock = messaging.pub_sock('logMessage') while True: dat = b''.join(sock.recv_multipart()) dat = dat.decode('utf8') # print "RECV", repr(dat) levelnum = ord(dat[0]) dat = dat[1:] if levelnum >= le_level: # push to logentries # TODO: push to athena instead le_handler.emit_raw(dat) # then we publish them msg = messaging.new_message() msg.logMessage = dat pub_sock.send(msg.to_bytes())
def __init__(self): #ALCA params self.ALCA_error = False self.ALCA_lane_width = 3.6 self.ALCA_direction = 100 #none 0, left 1, right -1,reset 100 self.ALCA_step = 0 self.ALCA_total_steps = 20 * ALCA_duration_seconds #20 Hz, 5 seconds, wifey mode self.ALCA_cancelling = False self.ALCA_enabled = False self.ALCA_OFFSET_C3 = 0. self.ALCA_OFFSET_C2 = 0. self.ALCA_OFFSET_C1 = 0. self.ALCA_over_line = False self.prev_CS_ALCA_error = False self.ALCA_use_visual = True self.ALCA_vego = 0. self.ALCA_vego_prev = 0. self.alcaStatus = messaging.sub_sock('alcaStatus', conflate=True) self.alcaState = messaging.pub_sock('alcaState') self.alcas = None self.hit_prob_low = False self.hit_prob_high = False self.distance_to_line_L = 100. self.prev_distance_to_line_L = 100. self.distance_to_line_R = 100. self.prev_distance_to_line_R = 100.
def test_steering_ipas(self): self.longMessage = True car_name = TOYOTA.RAV4 sendcan = messaging.pub_sock('sendcan') params = CarInterface.get_params(car_name) params.enableApgs = True CI = CarInterface(params, CarController) CI.CC.angle_control = True # Get parser parser_signals = [ ('SET_ME_X10', 'STEERING_IPAS', 0), ('SET_ME_X40', 'STEERING_IPAS', 0), ('ANGLE', 'STEERING_IPAS', 0), ('STATE', 'STEERING_IPAS', 0), ('DIRECTION_CMD', 'STEERING_IPAS', 0), ] parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") time.sleep(0.2) # Slow joiner syndrome for enabled in [True, False]: for steer in np.linspace(-510., 510., 25): control = car.CarControl.new_message() actuators = car.CarControl.Actuators.new_message() actuators.steerAngle = float(steer) control.enabled = enabled control.actuators = actuators CI.update(control) CI.CS.steer_not_allowed = False for _ in range(1000 if steer < -505 else 25): can_sends = CI.apply(control) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) parser.update(int(sec_since_boot() * 1e9), False) self.assertEqual(0x10, parser.vl['STEERING_IPAS']['SET_ME_X10']) self.assertEqual(0x40, parser.vl['STEERING_IPAS']['SET_ME_X40']) expected_state = 3 if enabled else 1 self.assertEqual(expected_state, parser.vl['STEERING_IPAS']['STATE']) if steer < 0: direction = 3 elif steer > 0: direction = 1 else: direction = 2 if not enabled: direction = 2 self.assertEqual(direction, parser.vl['STEERING_IPAS']['DIRECTION_CMD']) expected_steer = int(round(steer / 1.5)) * 1.5 if enabled else 0 self.assertAlmostEqual(expected_steer, parser.vl['STEERING_IPAS']['ANGLE']) sendcan.close()
def main(): ethernetData = messaging.pub_sock('ethernetData') for ts, pkt in pcap.pcap('eth0'): dat = messaging.new_message('ethernetData', 1) dat.ethernetData[0].ts = ts dat.ethernetData[0].pkt = str(pkt) ethernetData.send(dat.to_bytes())
def boardd_loop(rate=100): rk = Ratekeeper(rate) can_init() # *** publishes can and health logcan = messaging.pub_sock('can') health_sock = messaging.pub_sock('health') # *** subscribes to can send sendcan = messaging.sub_sock('sendcan') # drain sendcan to delete any stale messages from previous runs messaging.drain_sock(sendcan) while 1: # health packet @ 2hz if (rk.frame % (rate // 2)) == 0: health = can_health() msg = messaging.new_message() msg.init('health') # store the health to be logged msg.health.voltage = health['voltage'] msg.health.current = health['current'] msg.health.ignitionLine = health['ignition_line'] msg.health.ignitionCan = health['ignition_can'] msg.health.controlsAllowed = True health_sock.send(msg.to_bytes()) # recv @ 100hz can_msgs = can_recv() # publish to logger # TODO: refactor for speed if len(can_msgs) > 0: dat = can_list_to_can_capnp(can_msgs).to_bytes() logcan.send(dat) # send can if we have a packet tsc = messaging.recv_sock(sendcan) if tsc is not None: can_send_many(can_capnp_to_can_list(tsc.sendcan)) rk.keep_time()
def test_boardd_loopback(): # wait for boardd to init spinner = Spinner() time.sleep(2) with Timeout(60, "boardd didn't start"): sm = messaging.SubMaster(['pandaState']) while sm.rcv_frame['pandaState'] < 1: sm.update(1000) # boardd blocks on CarVin and CarParams cp = car.CarParams.new_message() cp.safetyModel = car.CarParams.SafetyModel.allOutput Params().put("CarVin", b"0" * 17) Params().put_bool("ControlsReady", True) Params().put("CarParams", cp.to_bytes()) sendcan = messaging.pub_sock('sendcan') can = messaging.sub_sock('can', conflate=False, timeout=100) time.sleep(1) n = 1000 for i in range(n): spinner.update(f"boardd loopback {i}/{n}") sent_msgs = defaultdict(set) for _ in range(random.randrange(10)): to_send = [] for __ in range(random.randrange(100)): bus = random.randrange(3) addr = random.randrange(1, 1 << 29) dat = bytes([ random.getrandbits(8) for _ in range(random.randrange(1, 9)) ]) sent_msgs[bus].add((addr, dat)) to_send.append(make_can_msg(addr, dat, bus)) sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) max_recv = 10 while max_recv > 0 and any(len(sent_msgs[bus]) for bus in range(3)): recvd = messaging.drain_sock(can, wait_for_one=True) for msg in recvd: for m in msg.can: if m.src >= 128: k = (m.address, m.dat) assert k in sent_msgs[m.src - 128] sent_msgs[m.src - 128].discard(k) max_recv -= 1 # if a set isn't empty, messages got dropped for bus in range(3): assert not len( sent_msgs[bus] ), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages" spinner.close()
def receiver_thread(): if PYGAME: pygame.init() pygame.display.set_caption("vnet debug UI") screen = pygame.display.set_mode((1164,874), pygame.DOUBLEBUF) camera_surface = pygame.surface.Surface((1164,874), 0, 24).convert() addr = "192.168.5.11" if len(sys.argv) >= 2: addr = sys.argv[1] context = zmq.Context() s = messaging.sub_sock(context, 9002, addr=addr) frame_sock = messaging.pub_sock(context, service_list['frame'].port) ctx = av.codec.codec.Codec('hevc', 'r').create() ctx.decode(av.packet.Packet(start.decode("hex"))) import time while 1: t1 = time.time() ts, raw = s.recv_multipart() ts = struct.unpack('q', ts)[0] * 1000 t1, t2 = time.time(), t1 #print 'ms to get frame:', (t1-t2)*1000 pkt = av.packet.Packet(raw) f = ctx.decode(pkt) if not f: continue f = f[0] t1, t2 = time.time(), t1 #print 'ms to decode:', (t1-t2)*1000 y_plane = np.frombuffer(f.planes[0], np.uint8).reshape((874, 1216))[:, 0:1164] u_plane = np.frombuffer(f.planes[1], np.uint8).reshape((437, 608))[:, 0:582] v_plane = np.frombuffer(f.planes[2], np.uint8).reshape((437, 608))[:, 0:582] yuv_img = y_plane.tobytes() + u_plane.tobytes() + v_plane.tobytes() t1, t2 = time.time(), t1 #print 'ms to make yuv:', (t1-t2)*1000 #print 'tsEof:', ts dat = messaging.new_message('frame') dat.frame.image = yuv_img dat.frame.timestampEof = ts dat.frame.transform = map(float, list(np.eye(3).flatten())) frame_sock.send(dat.to_bytes()) if PYGAME: yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(874 * 3 // 2, -1) cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff) #print yuv_np.shape, imgff.shape #scipy.misc.imsave("tmp.png", imgff) pygame.surfarray.blit_array(camera_surface, imgff.swapaxes(0,1)) screen.blit(camera_surface, (0, 0)) pygame.display.flip()
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: Plant.radar = messaging.pub_sock('radarState') Plant.controls_state = messaging.pub_sock('controlsState') Plant.car_state = messaging.pub_sock('carState') Plant.plan = messaging.sub_sock('longitudinalPlan') Plant.messaging_initialized = True self.v_lead_prev = 0.0 self.distance = 0. self.speed = speed self.acceleration = 0.0 # lead car self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy self.only_lead2 = only_lead2 self.only_radar = only_radar self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate time.sleep(1) self.sm = messaging.SubMaster(['longitudinalPlan']) # make sure planner has time to be fully initialized # TODO planner should just be explicitly initialized for i in range(10000): assert i < 10000 radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') car_state = messaging.new_message('carState') control.controlsState.longControlState = LongCtrlState.pid control.controlsState.vCruise = speed * 3.6 car_state.carState.vEgo = self.speed Plant.radar.send(radar.to_bytes()) Plant.controls_state.send(control.to_bytes()) Plant.car_state.send(car_state.to_bytes())
def send_deviceState(): messaging.context = messaging.Context() pub_sock = messaging.pub_sock("deviceState") start = time.time() while time.time() - start < 1: msg = messaging.new_message('deviceState') pub_sock.send(msg.to_bytes()) time.sleep(0.01)
def send_thermal(): messaging.context = messaging.Context() pub_sock = messaging.pub_sock("thermal") start = time.time() while time.time() - start < 1: msg = messaging.new_message('thermal') pub_sock.send(msg.to_bytes()) time.sleep(0.01)
def mock_lead(): radarState = messaging.pub_sock('radarState') while 1: m = messaging.new_message('radarState') m.radarState.leadOne.status = True for x in leadRange(3.0, 65.0, 0.005): m.radarState.leadOne.dRel = x radarState.send(m.to_bytes()) time.sleep(0.01)
def mock_x(): liveMpc = messaging.pub_sock('liveMpc') while 1: m = messaging.new_message('liveMpc') mx = [] for x in range(0, 100): mx.append(x * 1.0) m.liveMpc.x = mx liveMpc.send(m.to_bytes())
def mock(): traffic_events = messaging.pub_sock('uiNavigationEvent') while 1: m = messaging.new_message('uiNavigationEvent') m.uiNavigationEvent.type = log.UiNavigationEvent.Type.mergeRight m.uiNavigationEvent.status = log.UiNavigationEvent.Status.active m.uiNavigationEvent.distanceTo = 100. traffic_events.send(m.to_bytes()) time.sleep(0.01)
def test_getitem(self): sock = "carState" pub_sock = messaging.pub_sock(sock) sm = messaging.SubMaster([sock,]) zmq_sleep() msg = random_carstate() pub_sock.send(msg.to_bytes()) sm.update(1000) assert_carstate(msg.carState, sm[sock])
def main(): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} for i in range(1, 33): nav_frame_buffer[0][i] = {} gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') ubloxGnss = messaging.pub_sock('ubloxGnss') dev = init_reader() while True: try: msg = dev.receive_message() except serial.serialutil.SerialException as e: print(e) dev.close() dev = init_reader() if msg is not None: handle_msg(dev, msg, nav_frame_buffer)