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()
|