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)
