369 lines
15 KiB
Python
369 lines
15 KiB
Python
import sqlite3
|
||
import json
|
||
import os
|
||
from http.server import HTTPServer, BaseHTTPRequestHandler
|
||
|
||
DB_PATH = os.path.expanduser('~/inertial_control/inertial_data.db')
|
||
PORT = 8050
|
||
TRAJ_LIMIT = 5000 # показываем последние 5000 точек траектории (~60 сек движения)
|
||
LIDAR_LIMIT = 2000 # последние точки лидара
|
||
|
||
HTML = """
|
||
<!DOCTYPE html>
|
||
<html>
|
||
<head>
|
||
<title>Inertial Tracker — Реальное время</title>
|
||
<script src="plotly-latest.min.js"></script>
|
||
<style>
|
||
* { box-sizing: border-box; }
|
||
body { margin:0; background:#111827; font-family: 'Courier New', monospace; }
|
||
|
||
#topbar {
|
||
position: fixed; top: 0; left: 0; right: 0; height: 48px;
|
||
background: #1f2937; border-bottom: 1px solid #374151;
|
||
display: flex; align-items: center; padding: 0 16px; gap: 24px;
|
||
z-index: 200;
|
||
}
|
||
#topbar .title { color: #f9fafb; font-size: 15px; font-weight: bold; letter-spacing: 1px; }
|
||
#topbar .badge {
|
||
padding: 3px 10px; border-radius: 12px; font-size: 12px; font-weight: bold;
|
||
}
|
||
.badge-moving { background: #065f46; color: #6ee7b7; }
|
||
.badge-still { background: #1e3a5f; color: #93c5fd; }
|
||
.badge-nodata { background: #3f2a2a; color: #fca5a5; }
|
||
#topbar .dist { color: #9ca3af; font-size: 13px; }
|
||
#topbar .dist span { color: #e5e7eb; }
|
||
|
||
#coords {
|
||
position: fixed; top: 60px; right: 12px;
|
||
background: #1f2937; color: #6ee7b7;
|
||
padding: 12px 16px; font-size: 17px; z-index: 100;
|
||
border-radius: 10px; border: 1px solid #374151;
|
||
line-height: 1.8;
|
||
}
|
||
#legend {
|
||
position: fixed; top: 60px; left: 12px;
|
||
background: #1f2937; color: #d1d5db;
|
||
padding: 10px 14px; font-size: 13px; z-index: 100;
|
||
border-radius: 10px; border: 1px solid #374151;
|
||
line-height: 2.0;
|
||
}
|
||
#hint {
|
||
position: fixed; bottom: 42px; left: 12px;
|
||
background: #1f2937; color: #6b7280;
|
||
padding: 7px 12px; font-size: 11px; z-index: 100;
|
||
border-radius: 8px; border: 1px solid #374151;
|
||
max-width: 260px; line-height: 1.5;
|
||
}
|
||
#status {
|
||
position: fixed; bottom: 10px; left: 12px;
|
||
background: #111827; color: #6b7280;
|
||
padding: 4px 10px; font-size: 11px; z-index: 100;
|
||
border-radius: 6px;
|
||
}
|
||
#plot { width: 100%; height: 100vh; padding-top: 48px; }
|
||
</style>
|
||
</head>
|
||
<body>
|
||
<div id="topbar">
|
||
<div class="title">INERTIAL TRACKER</div>
|
||
<div id="motion-badge" class="badge badge-nodata">Нет данных</div>
|
||
<div class="dist">Пройдено: <span id="dist-val">0.00</span> м</div>
|
||
<div class="dist">Время: <span id="time-val">0:00</span></div>
|
||
</div>
|
||
|
||
<div id="coords">X: —<br>Y: —<br>Z: —</div>
|
||
|
||
<div id="legend">
|
||
<span style="color:#ffffff; font-size:16px;">◆</span> Начальная точка (0, 0, 0)<br>
|
||
<span style="color:#f87171; font-size:14px;">━━</span> Траектория движения<br>
|
||
<span style="color:#60a5fa; font-size:10px;">●●●</span> Карта окружения (лидар)<br>
|
||
<span style="color:#34d399; font-size:16px;">●</span> Текущее положение
|
||
</div>
|
||
|
||
<div id="hint">
|
||
Оси X, Y, Z — перемещение в метрах.<br>
|
||
Масштаб одинаковый по всем осям.<br>
|
||
Вращайте сцену мышью.
|
||
</div>
|
||
|
||
<div id="status">Подключение...</div>
|
||
<div id="plot"></div>
|
||
|
||
<script>
|
||
let updateCount = 0;
|
||
let lastRange = null;
|
||
let totalDist = 0;
|
||
let lastPos = null;
|
||
let startTime = null;
|
||
let sessionTimer = null;
|
||
|
||
// Вычисляет равные диапазоны для всех трёх осей
|
||
// (одинаковый масштаб = квадратный куб, без растяжки по Z)
|
||
function squareRanges(allX, allY, allZ) {
|
||
function minMax(arr) {
|
||
if (!arr || arr.length === 0) return [0, 0];
|
||
let mn = arr[0], mx = arr[0];
|
||
for (let v of arr) { if (v < mn) mn = v; if (v > mx) mx = v; }
|
||
return [mn, mx];
|
||
}
|
||
const rx = minMax(allX), ry = minMax(allY), rz = minMax(allZ);
|
||
const spans = [rx[1]-rx[0], ry[1]-ry[0], rz[1]-rz[0]];
|
||
const maxSpan = Math.max(...spans, 0.5); // минимум 0.5 м
|
||
const half = maxSpan / 2 * 1.2; // запас 20%
|
||
|
||
function mid(r) { return (r[0] + r[1]) / 2; }
|
||
return [
|
||
[mid(rx) - half, mid(rx) + half],
|
||
[mid(ry) - half, mid(ry) + half],
|
||
[mid(rz) - half, mid(rz) + half]
|
||
];
|
||
}
|
||
|
||
function rangeChanged(newR, oldR) {
|
||
if (!oldR) return true;
|
||
for (let i = 0; i < newR.length; i++) {
|
||
if (Math.abs(newR[i][0] - oldR[i][0]) > 0.25) return true;
|
||
if (Math.abs(newR[i][1] - oldR[i][1]) > 0.25) return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
function fmtTime(sec) {
|
||
const m = Math.floor(sec / 60), s = Math.floor(sec % 60);
|
||
return m + ':' + String(s).padStart(2, '0');
|
||
}
|
||
|
||
// Инициализация графика
|
||
Plotly.newPlot('plot', [
|
||
{
|
||
name: 'Начало',
|
||
x: [0], y: [0], z: [0],
|
||
mode: 'markers',
|
||
type: 'scatter3d',
|
||
marker: { size: 10, color: 'white', symbol: 'diamond',
|
||
line: { color: '#374151', width: 2 } }
|
||
},
|
||
{
|
||
name: 'Траектория',
|
||
x: [], y: [], z: [],
|
||
mode: 'lines',
|
||
type: 'scatter3d',
|
||
line: { color: '#f87171', width: 3 }
|
||
},
|
||
{
|
||
name: 'Лидар',
|
||
x: [], y: [], z: [],
|
||
mode: 'markers',
|
||
type: 'scatter3d',
|
||
marker: { size: 2, color: '#60a5fa', opacity: 0.5 }
|
||
},
|
||
{
|
||
name: 'Позиция',
|
||
x: [0], y: [0], z: [0],
|
||
mode: 'markers',
|
||
type: 'scatter3d',
|
||
marker: { size: 12, color: '#34d399', symbol: 'circle',
|
||
line: { color: '#6ee7b7', width: 2 } }
|
||
}
|
||
], {
|
||
scene: {
|
||
xaxis: { title: 'X (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
|
||
yaxis: { title: 'Y (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
|
||
zaxis: { title: 'Z (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
|
||
bgcolor: '#111827',
|
||
aspectmode: 'cube',
|
||
camera: { eye: { x: 1.4, y: 1.4, z: 1.0 } }
|
||
},
|
||
paper_bgcolor: '#111827',
|
||
plot_bgcolor: '#111827',
|
||
showlegend: false,
|
||
margin: { l: 0, r: 0, b: 0, t: 0 }
|
||
});
|
||
|
||
async function update() {
|
||
try {
|
||
const r = await fetch('/api/data?t=' + Date.now());
|
||
if (!r.ok) throw new Error('HTTP ' + r.status);
|
||
const d = await r.json();
|
||
updateCount++;
|
||
|
||
const pos = d.pos;
|
||
const hasData = d.traj.x.length > 0;
|
||
|
||
// Координаты
|
||
document.getElementById('coords').innerHTML =
|
||
`X: ${pos.x.toFixed(3)} м<br>Y: ${pos.y.toFixed(3)} м<br>Z: ${pos.z.toFixed(3)} м`;
|
||
|
||
// Таймер сессии
|
||
if (hasData && !startTime) {
|
||
startTime = Date.now();
|
||
sessionTimer = setInterval(() => {
|
||
document.getElementById('time-val').textContent =
|
||
fmtTime((Date.now() - startTime) / 1000);
|
||
}, 1000);
|
||
}
|
||
|
||
// Пройденное расстояние
|
||
if (lastPos && hasData) {
|
||
const dx = pos.x - lastPos.x,
|
||
dy = pos.y - lastPos.y,
|
||
dz = pos.z - lastPos.z;
|
||
const step = Math.sqrt(dx*dx + dy*dy + dz*dz);
|
||
if (step < 0.5) totalDist += step; // фильтр прыжков
|
||
}
|
||
lastPos = pos;
|
||
document.getElementById('dist-val').textContent = totalDist.toFixed(2);
|
||
|
||
// Статус движения
|
||
const badge = document.getElementById('motion-badge');
|
||
if (!hasData) {
|
||
badge.textContent = 'Нет данных';
|
||
badge.className = 'badge badge-nodata';
|
||
} else {
|
||
const spd = lastPos ? Math.sqrt(
|
||
(pos.x-lastPos.x)**2 + (pos.y-lastPos.y)**2 + (pos.z-lastPos.z)**2
|
||
) : 0;
|
||
if (totalDist > 0.02 && spd > 0.001) {
|
||
badge.textContent = '▶ В движении';
|
||
badge.className = 'badge badge-moving';
|
||
} else {
|
||
badge.textContent = '■ В покое';
|
||
badge.className = 'badge badge-still';
|
||
}
|
||
}
|
||
|
||
// Обновляем трассы
|
||
Plotly.restyle('plot', { x:[d.traj.x], y:[d.traj.y], z:[d.traj.z] }, 1);
|
||
Plotly.restyle('plot', { x:[d.lidar.x], y:[d.lidar.y], z:[d.lidar.z] }, 2);
|
||
Plotly.restyle('plot', { x:[[pos.x]], y:[[pos.y]], z:[[pos.z]] }, 3);
|
||
|
||
// Автоподгонка с одинаковым масштабом по всем осям
|
||
const allX = [0, pos.x].concat(d.traj.x);
|
||
const allY = [0, pos.y].concat(d.traj.y);
|
||
const allZ = [0, pos.z].concat(d.traj.z);
|
||
const newRange = squareRanges(allX, allY, allZ);
|
||
if (rangeChanged(newRange, lastRange)) {
|
||
lastRange = newRange;
|
||
Plotly.relayout('plot', {
|
||
'scene.xaxis.range': newRange[0],
|
||
'scene.yaxis.range': newRange[1],
|
||
'scene.zaxis.range': newRange[2]
|
||
});
|
||
}
|
||
|
||
document.getElementById('status').textContent =
|
||
`Обновлений: ${updateCount} | Траектория: ${d.traj.x.length} тч | Лидар: ${d.lidar.x.length} тч | ${new Date().toLocaleTimeString()}`;
|
||
|
||
} catch(e) {
|
||
document.getElementById('status').textContent = 'Ошибка: ' + e.message;
|
||
}
|
||
}
|
||
|
||
update();
|
||
setInterval(update, 200);
|
||
</script>
|
||
</body>
|
||
</html>
|
||
"""
|
||
|
||
|
||
def fetch_data():
|
||
"""Читает свежие данные из БД в read-only режиме (не мешает писателям)."""
|
||
conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
|
||
try:
|
||
# Текущая позиция
|
||
cur = conn.execute(
|
||
"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1"
|
||
)
|
||
row = cur.fetchone()
|
||
pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||
|
||
# Последние N точек траектории (разворачиваем в хронологический порядок)
|
||
cur = conn.execute(
|
||
f"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT {TRAJ_LIMIT}"
|
||
)
|
||
traj_rows = cur.fetchall()[::-1]
|
||
traj = {
|
||
'x': [r[0] for r in traj_rows],
|
||
'y': [r[1] for r in traj_rows],
|
||
'z': [r[2] for r in traj_rows],
|
||
}
|
||
|
||
# Последние N точек лидара
|
||
cur = conn.execute(
|
||
f"SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT {LIDAR_LIMIT}"
|
||
)
|
||
lidar_rows = cur.fetchall()
|
||
lidar = {
|
||
'x': [r[0] for r in lidar_rows],
|
||
'y': [r[1] for r in lidar_rows],
|
||
'z': [r[2] for r in lidar_rows],
|
||
}
|
||
finally:
|
||
conn.close()
|
||
return {'pos': pos, 'traj': traj, 'lidar': lidar}
|
||
|
||
|
||
class Handler(BaseHTTPRequestHandler):
|
||
def log_message(self, format, *args):
|
||
pass
|
||
|
||
def do_GET(self):
|
||
if self.path == '/':
|
||
self.send_response(200)
|
||
self.send_header('Content-type', 'text/html; charset=utf-8')
|
||
self.end_headers()
|
||
self.wfile.write(HTML.encode('utf-8'))
|
||
|
||
elif self.path == '/plotly-latest.min.js':
|
||
self.send_response(200)
|
||
self.send_header('Content-type', 'application/javascript')
|
||
self.end_headers()
|
||
with open('plotly-latest.min.js', 'rb') as f:
|
||
self.wfile.write(f.read())
|
||
|
||
elif self.path.startswith('/api/data'):
|
||
try:
|
||
data = fetch_data()
|
||
payload = json.dumps(data).encode('utf-8')
|
||
self.send_response(200)
|
||
self.send_header('Content-type', 'application/json')
|
||
self.send_header('Cache-Control', 'no-store')
|
||
self.end_headers()
|
||
self.wfile.write(payload)
|
||
except Exception as e:
|
||
self.send_response(500)
|
||
self.end_headers()
|
||
self.wfile.write(str(e).encode('utf-8'))
|
||
|
||
elif self.path.startswith('/api/pos'):
|
||
try:
|
||
conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
|
||
cur = conn.execute(
|
||
"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1"
|
||
)
|
||
row = cur.fetchone()
|
||
conn.close()
|
||
pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0, 'y': 0, 'z': 0}
|
||
self.send_response(200)
|
||
self.send_header('Content-type', 'application/json')
|
||
self.end_headers()
|
||
self.wfile.write(json.dumps(pos).encode('utf-8'))
|
||
except Exception as e:
|
||
self.send_response(500)
|
||
self.end_headers()
|
||
self.wfile.write(str(e).encode('utf-8'))
|
||
|
||
else:
|
||
self.send_response(404)
|
||
self.end_headers()
|
||
|
||
|
||
if __name__ == '__main__':
|
||
print("=" * 50)
|
||
print("INERTIAL TRACKER — реальное время")
|
||
print(f"Открыть: http://192.168.0.106:{PORT}")
|
||
print("=" * 50)
|
||
HTTPServer(('0.0.0.0', PORT), Handler).serve_forever()
|