forked from udacity/CarND-Unscented-Kalman-Filter-Project
-
Notifications
You must be signed in to change notification settings - Fork 1
/
kalman_tracker.py
102 lines (82 loc) · 2.53 KB
/
kalman_tracker.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
#_______________________________________________________________________________
# kalman_tracker.py 80->|
# This module implements the socket linkage to the Unity visualizer.
# Modified to pass an extra argument to the Kalman filter executable.
#
import argparse
import base64
from datetime import datetime
import os
import shutil
import numpy as np
import socketio
import eventlet
import eventlet.wsgi
from flask import Flask
from io import BytesIO
from subprocess import call
from subprocess import check_output
import csv
sio = socketio.Server()
app = Flask(__name__)
model = None
def get_last_row(csv_filename):
with open(csv_filename) as tsv:
lastrow = None
for lastrow in csv.reader(tsv, dialect="excel-tab"): pass
return lastrow
@sio.on('telemetry')
def telemetry(sid, data):
if data:
# add extra argument to process only last 10 measurements
output = check_output([model,"data_in.txt","data_out.txt","/viz"])
lastrow = get_last_row("data_out.txt")
x_markers = lastrow[0]
y_markers = lastrow[1]
#print("Sending: "+x_markers+" , "+y_markers)
output = output.decode("utf-8")
#print(output)
outputVals = output.split(" ") # <-- use space as delimiter
if outputVals[0].find("RMSE") != -1:
send_estimate_rmse(x_markers,y_markers, outputVals[1],outputVals[2],outputVals[3],outputVals[4])
else:
send_estimate(x_markers,y_markers)
else:
# NOTE: DON'T EDIT THIS.
sio.emit('manual', data={}, skip_sid=True)
@sio.on('connect')
def connect(sid, environ):
print("connect ", sid)
def send_estimate(estimate_x, estimate_y):
sio.emit(
"estimate",
data={
'estimate_x': estimate_x,
'estimate_y': estimate_y
},
skip_sid=True)
def send_estimate_rmse(estimate_x, estimate_y, rmse_x, rmse_y, rmse_vx, rmse_vy):
sio.emit(
"estimate_rmse",
data={
'estimate_x': estimate_x,
'estimate_y': estimate_y,
'rmse_x': rmse_x,
'rmse_y': rmse_y,
'rmse_vx': rmse_vx,
'rmse_vy': rmse_vy
},
skip_sid=True)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Kalman Filter Tracker')
parser.add_argument(
'model',
type=str,
help='Path to Kalman filter executable'
)
args = parser.parse_args()
model = args.model
# wrap Flask application with engineio's middleware
app = socketio.Middleware(sio, app)
# deploy as an eventlet WSGI server
eventlet.wsgi.server(eventlet.listen(('', 4567)), app)