You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
49 lines
1.4 KiB
49 lines
1.4 KiB
6 years ago
|
#!/usr/bin/env python3
|
||
|
|
import time
|
||
|
|
import sys
|
||
|
|
import argparse
|
||
|
|
import zmq
|
||
|
|
import json
|
||
|
|
import pyproj
|
||
|
|
import numpy as np
|
||
|
|
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
|
||
|
|
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
|
||
|
|
|
||
|
|
import cereal.messaging as messaging
|
||
|
|
from cereal.services import service_list
|
||
|
|
|
||
|
|
poller = zmq.Poller()
|
||
|
|
ll = messaging.sub_sock("liveLocation", poller)
|
||
|
|
tll = messaging.sub_sock("testLiveLocation", poller)
|
||
|
|
|
||
|
|
l, tl = None, None
|
||
|
|
|
||
|
|
lp = time.time()
|
||
|
|
|
||
|
|
while 1:
|
||
|
|
polld = poller.poll(timeout=1000)
|
||
|
|
for sock, mode in polld:
|
||
|
|
if mode != zmq.POLLIN:
|
||
|
|
continue
|
||
|
|
if sock == ll:
|
||
|
|
l = messaging.recv_one(sock)
|
||
|
|
elif sock == tll:
|
||
|
|
tl = messaging.recv_one(sock)
|
||
|
|
if l is None or tl is None:
|
||
|
|
continue
|
||
|
|
|
||
|
|
alt_err = np.abs(l.liveLocation.alt - tl.liveLocation.alt)
|
||
|
|
l1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt)
|
||
|
|
l2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, tl.liveLocation.alt)
|
||
|
|
|
||
|
|
al1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt)
|
||
|
|
al2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, l.liveLocation.alt)
|
||
|
|
|
||
|
|
tdiff = np.abs(l.logMonoTime - tl.logMonoTime) / 1e9
|
||
|
|
|
||
|
|
if time.time()-lp > 0.1:
|
||
|
|
print("tm: %f mse: %f mse(flat): %f alterr: %f" % (tdiff, np.mean((np.array(l1)-np.array(l2))**2), np.mean((np.array(al1)-np.array(al2))**2), alt_err))
|
||
|
|
lp = time.time()
|
||
|
|
|
||
|
|
|