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.
99 lines
2.7 KiB
99 lines
2.7 KiB
6 years ago
|
#!/usr/bin/env python
|
||
|
|
from common.kalman.ned import ecef2geodetic
|
||
|
|
|
||
|
|
import csv
|
||
|
|
import numpy as np
|
||
|
|
import webbrowser
|
||
|
|
import os
|
||
|
|
import sys
|
||
|
|
import json
|
||
|
|
import numpy.linalg as LA
|
||
|
|
import gmplot
|
||
|
|
from dateutil.parser import parse
|
||
|
|
from common.numpy_helpers import deep_interp
|
||
|
|
# import cvxpy as cvx
|
||
|
|
MPH_TO_MS = 0.44704
|
||
|
|
|
||
|
|
|
||
|
|
def downsample(positions, speeds, start_idx, end_idx, dist):
|
||
|
|
# TODO: save headings too
|
||
|
|
track = []
|
||
|
|
last_position = positions[start_idx]
|
||
|
|
valid_indeces = []
|
||
|
|
track_speeds = []
|
||
|
|
for pi in range(start_idx, end_idx):
|
||
|
|
# only save points that are at least 10 cm far away
|
||
|
|
if LA.norm(positions[pi] - last_position) >= dist:
|
||
|
|
#print LA.norm(positions[pi] - last_position)
|
||
|
|
last_position = positions[pi]
|
||
|
|
track.append(positions[pi])
|
||
|
|
valid_indeces.append(pi)
|
||
|
|
track_speeds.append(speeds[pi])
|
||
|
|
print(-start_idx + end_idx, len(valid_indeces))
|
||
|
|
# this compare the original point count vs the filtered count
|
||
|
|
|
||
|
|
track = np.array(track)
|
||
|
|
track_speeds = np.array(track_speeds)
|
||
|
|
return track, track_speeds
|
||
|
|
|
||
|
|
def converter(date):
|
||
|
|
|
||
|
|
filename = "/home/batman/one/selfdrive/locationd/liveloc_dumps/" + date + "/canonical.csv" # Point one (OK!)
|
||
|
|
|
||
|
|
c = csv.DictReader(open(filename, 'rb'), delimiter=',')
|
||
|
|
|
||
|
|
start_time = None
|
||
|
|
|
||
|
|
t = []
|
||
|
|
ll_positions = []
|
||
|
|
positions = []
|
||
|
|
sats = []
|
||
|
|
flag = []
|
||
|
|
speeds = []
|
||
|
|
|
||
|
|
for row in c:
|
||
|
|
t.append(float(row['pctime']))
|
||
|
|
x = float(row['ecefX'])
|
||
|
|
y = float(row['ecefY'])
|
||
|
|
z = float(row['ecefZ'])
|
||
|
|
ecef = np.array((x, y, z))
|
||
|
|
speeds.append(float(row['velSpeed']))
|
||
|
|
|
||
|
|
pos = ecef2geodetic(ecef)
|
||
|
|
ll_positions.append(pos)
|
||
|
|
positions.append(ecef)
|
||
|
|
|
||
|
|
t = np.array(t)
|
||
|
|
ll_positions = np.array(ll_positions)
|
||
|
|
positions = np.array(positions)
|
||
|
|
|
||
|
|
#distances = ll_positions[:,0:2] - START_POS[:2]
|
||
|
|
#i_start = np.argmin(LA.norm(distances, axis=1))
|
||
|
|
|
||
|
|
#for i in range(i_start + 500):
|
||
|
|
# distances[i] += np.array([100, 100])
|
||
|
|
#i_end = np.argmin(LA.norm(distances, axis=1))
|
||
|
|
|
||
|
|
i_start = 0
|
||
|
|
i_end = len(positions)
|
||
|
|
|
||
|
|
print(i_start, i_end)
|
||
|
|
track, track_speeds = downsample(positions, speeds, i_start, i_end, 0.2)
|
||
|
|
ll_track = np.array([ecef2geodetic(pos) for pos in track])
|
||
|
|
|
||
|
|
track_struct = {}
|
||
|
|
print(track_speeds.shape)
|
||
|
|
print(track.shape)
|
||
|
|
track_struct['race'] = np.hstack((track,
|
||
|
|
np.expand_dims(track_speeds, axis=1),
|
||
|
|
np.zeros((len(track_speeds), 1))))
|
||
|
|
|
||
|
|
f = open('/home/batman/one/selfdrive/controls/tracks/loop_city.npy', 'w')
|
||
|
|
np.save(f, track_struct)
|
||
|
|
f.close()
|
||
|
|
print("SAVED!")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
converter(sys.argv[1])
|