453 lines
19 KiB
Python
453 lines
19 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
|
||
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:#0f0f0f; font-family: 'Courier New', monospace; }
|
||
|
||
#topbar {
|
||
position: fixed; top: 0; left: 0; right: 0; height: 48px;
|
||
background: #181818; border-bottom: 1px solid #2a2a2a;
|
||
display: flex; align-items: center; padding: 0 16px; gap: 24px;
|
||
z-index: 200;
|
||
}
|
||
#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: #181818; color: #6ee7b7;
|
||
padding: 12px 16px; font-size: 17px; z-index: 100;
|
||
border-radius: 10px; border: 1px solid #2a2a2a;
|
||
line-height: 1.8;
|
||
}
|
||
#legend {
|
||
position: fixed; top: 60px; left: 12px;
|
||
background: #181818; color: #d1d5db;
|
||
padding: 10px 14px; font-size: 13px; z-index: 100;
|
||
border-radius: 10px; border: 1px solid #2a2a2a;
|
||
line-height: 2.0;
|
||
}
|
||
#status {
|
||
position: fixed; bottom: 6px; left: 12px;
|
||
background: #0f0f0f; color: #4b5563;
|
||
padding: 4px 10px; font-size: 11px; z-index: 100;
|
||
border-radius: 6px;
|
||
}
|
||
|
||
#main-wrap {
|
||
position: fixed; top: 48px; left: 0; right: 0; bottom: 0;
|
||
display: flex; flex-direction: column;
|
||
}
|
||
#plot3d { flex: 0 0 62%; width: 100%; }
|
||
#plots2d {
|
||
flex: 1 1 0; display: flex; flex-direction: row;
|
||
border-top: 1px solid #2a2a2a;
|
||
}
|
||
.plot2d { flex: 1 1 0; min-width: 0; }
|
||
.plot2d:not(:last-child) { border-right: 1px solid #2a2a2a; }
|
||
</style>
|
||
</head>
|
||
<body>
|
||
<div id="topbar">
|
||
<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:#ffffff;">●</span> Положение робота<br>
|
||
<span style="color:#fbbf24;">━━</span> Направление (жёлтый луч)
|
||
</div>
|
||
|
||
<div id="status">Подключение...</div>
|
||
|
||
<div id="main-wrap">
|
||
<div id="plot3d"></div>
|
||
<div id="plots2d">
|
||
<div id="plot-xy" class="plot2d"></div>
|
||
<div id="plot-yz" class="plot2d"></div>
|
||
<div id="plot-zx" class="plot2d"></div>
|
||
</div>
|
||
</div>
|
||
|
||
<script>
|
||
let updateCount = 0;
|
||
let lastRange = null;
|
||
let totalDist = 0;
|
||
let lastPos = null;
|
||
let startTime = null;
|
||
let sessionTimer = null;
|
||
|
||
const BG = '#0f0f0f';
|
||
const GRID = '#1e1e1e';
|
||
const ZERO = '#2a2a2a';
|
||
const AXIS_COLOR = '#555';
|
||
const ARROW_LEN = 0.5; // длина жёлтого луча (м)
|
||
|
||
// ── Матрица полного поворота ─────────────────────────────────────────
|
||
function rotFull(roll, pitch, yaw) {
|
||
const cr=Math.cos(roll), sr=Math.sin(roll);
|
||
const cp=Math.cos(pitch), sp=Math.sin(pitch);
|
||
const cy=Math.cos(yaw), sy=Math.sin(yaw);
|
||
return [
|
||
[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
|
||
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
|
||
[-sp, cp*sr, cp*cr]
|
||
];
|
||
}
|
||
|
||
// Применение матрицы поворота к вектору
|
||
function applyRot(R, v) {
|
||
return [
|
||
R[0][0]*v[0] + R[0][1]*v[1] + R[0][2]*v[2],
|
||
R[1][0]*v[0] + R[1][1]*v[1] + R[1][2]*v[2],
|
||
R[2][0]*v[0] + R[2][1]*v[1] + R[2][2]*v[2]
|
||
];
|
||
}
|
||
|
||
// ── Вспомогательные функции ──────────────────────────────────────────
|
||
function minMax(arr) {
|
||
if (!arr || !arr.length) return [0,0];
|
||
let mn=arr[0], mx=arr[0];
|
||
for (const v of arr) { if(v<mn)mn=v; if(v>mx)mx=v; }
|
||
return [mn, mx];
|
||
}
|
||
function squareRanges(ax, ay, az) {
|
||
const rx=minMax(ax), ry=minMax(ay), rz=minMax(az);
|
||
const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],rz[1]-rz[0],0.5);
|
||
const half=maxSpan/2*1.2;
|
||
const mid=r=>(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(nR, oR) {
|
||
if (!oR) return true;
|
||
for (let i=0;i<nR.length;i++) {
|
||
if(Math.abs(nR[i][0]-oR[i][0])>0.25||Math.abs(nR[i][1]-oR[i][1])>0.25) return true;
|
||
}
|
||
return false;
|
||
}
|
||
function square2D(ax, ay) {
|
||
const rx=minMax(ax), ry=minMax(ay);
|
||
const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],0.5);
|
||
const half=maxSpan/2*1.2;
|
||
const mid=r=>(r[0]+r[1])/2;
|
||
return [[mid(rx)-half,mid(rx)+half],[mid(ry)-half,mid(ry)+half]];
|
||
}
|
||
function fmtTime(sec) {
|
||
const m=Math.floor(sec/60), s=Math.floor(sec%60);
|
||
return m+':'+String(s).padStart(2,'0');
|
||
}
|
||
|
||
const darkAxis3D = label => ({
|
||
title:label, color:AXIS_COLOR,
|
||
gridcolor:GRID, zerolinecolor:ZERO,
|
||
backgroundcolor: BG, showbackground: true
|
||
});
|
||
const darkAxis2D = label => ({
|
||
title:label, color:AXIS_COLOR,
|
||
gridcolor:GRID, zerolinecolor:ZERO,
|
||
tickfont:{color:AXIS_COLOR}
|
||
});
|
||
const darkLayout2D = (xL, yL) => ({
|
||
xaxis: darkAxis2D(xL), yaxis: darkAxis2D(yL),
|
||
paper_bgcolor: BG, plot_bgcolor: BG,
|
||
showlegend: false,
|
||
margin:{l:44,r:10,b:36,t:24},
|
||
font:{color:AXIS_COLOR, size:11}
|
||
});
|
||
|
||
// ── Инициализация 3D-графика ─────────────────────────────────────────
|
||
// Трейсы:
|
||
// 0 – начальная точка (белый ромб)
|
||
// 1 – траектория (красная линия)
|
||
// 2 – лидар (голубые точки)
|
||
// 3 – кружок – текущее положение (белый круг)
|
||
// 4 – жёлтый луч направления (линия)
|
||
Plotly.newPlot('plot3d', [
|
||
{
|
||
name:'Начало', x:[0],y:[0],z:[0],
|
||
mode:'markers', type:'scatter3d',
|
||
marker:{size:10,color:'white',symbol:'diamond',line:{color:'#2a2a2a',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:'white',symbol:'circle',line:{color:'#fbbf24',width:2}}
|
||
},
|
||
{
|
||
name:'Направление', x:[0,0], y:[0,0], z:[0,0],
|
||
mode:'lines', type:'scatter3d',
|
||
line:{color:'#fbbf24',width:4},
|
||
hoverinfo:'none'
|
||
}
|
||
], {
|
||
scene: {
|
||
xaxis: darkAxis3D('X (м)'),
|
||
yaxis: darkAxis3D('Y (м)'),
|
||
zaxis: darkAxis3D('Z (м)'),
|
||
bgcolor: BG,
|
||
aspectmode: 'cube',
|
||
camera: {eye:{x:1.4,y:1.4,z:1.0}}
|
||
},
|
||
paper_bgcolor: BG,
|
||
plot_bgcolor: BG,
|
||
showlegend: false,
|
||
margin:{l:0,r:0,b:0,t:0}
|
||
}, {responsive:true});
|
||
|
||
// ── 2D проекции ───────────────────────────────────────────────────────
|
||
function init2D(divId, xL, yL) {
|
||
Plotly.newPlot(divId, [
|
||
{ x:[0],y:[0], mode:'markers', type:'scatter',
|
||
marker:{size:8,color:'white',symbol:'diamond',line:{color:'#2a2a2a',width:1}} },
|
||
{ x:[],y:[], mode:'lines', type:'scatter',
|
||
line:{color:'#f87171',width:2} },
|
||
{ x:[0],y:[0], mode:'markers', type:'scatter',
|
||
marker:{size:10,color:'#34d399',line:{color:'#6ee7b7',width:2}} }
|
||
], darkLayout2D(xL,yL), {responsive:true});
|
||
}
|
||
init2D('plot-xy','X (м)','Y (м)');
|
||
init2D('plot-yz','Y (м)','Z (м)');
|
||
init2D('plot-zx','Z (м)','X (м)');
|
||
|
||
// ── Главный цикл обновления ────────────────────────────────────────────
|
||
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 if (totalDist>0.02) {
|
||
badge.textContent='▶ В движении'; badge.className='badge badge-moving';
|
||
} else {
|
||
badge.textContent='■ В покое'; badge.className='badge badge-still';
|
||
}
|
||
|
||
// Обновляем траекторию и лидар
|
||
Plotly.restyle('plot3d',{x:[d.traj.x],y:[d.traj.y],z:[d.traj.z]},1);
|
||
Plotly.restyle('plot3d',{x:[d.lidar.x],y:[d.lidar.y],z:[d.lidar.z]},2);
|
||
|
||
// ── Кружок (текущее положение) ────────────────────────────────
|
||
Plotly.restyle('plot3d',{x:[pos.x], y:[pos.y], z:[pos.z]}, 3);
|
||
|
||
// ── Жёлтый луч направления ────────────────────────────────────
|
||
const roll = d.orient.roll;
|
||
const pitch = d.orient.pitch;
|
||
const yaw = d.orient.yaw;
|
||
const R = rotFull(roll, pitch, yaw);
|
||
const dir = applyRot(R, [ARROW_LEN, 0, 0]); // вектор вперёд
|
||
const endX = pos.x + dir[0];
|
||
const endY = pos.y + dir[1];
|
||
const endZ = pos.z + dir[2];
|
||
Plotly.restyle('plot3d',{
|
||
x:[[pos.x, endX]], y:[[pos.y, endY]], z:[[pos.z, endZ]]
|
||
}, 4);
|
||
|
||
// Автомасштаб 3D
|
||
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('plot3d',{
|
||
'scene.xaxis.range':newRange[0],
|
||
'scene.yaxis.range':newRange[1],
|
||
'scene.zaxis.range':newRange[2]
|
||
});
|
||
}
|
||
|
||
// 2D проекции
|
||
const tx=d.traj.x, ty=d.traj.y, tz=d.traj.z;
|
||
|
||
Plotly.restyle('plot-xy',{x:[tx],y:[ty]},1);
|
||
Plotly.restyle('plot-xy',{x:[[pos.x]],y:[[pos.y]]},2);
|
||
const rXY=square2D([0,pos.x].concat(tx),[0,pos.y].concat(ty));
|
||
Plotly.relayout('plot-xy',{'xaxis.range':rXY[0],'yaxis.range':rXY[1]});
|
||
|
||
Plotly.restyle('plot-yz',{x:[ty],y:[tz]},1);
|
||
Plotly.restyle('plot-yz',{x:[[pos.y]],y:[[pos.z]]},2);
|
||
const rYZ=square2D([0,pos.y].concat(ty),[0,pos.z].concat(tz));
|
||
Plotly.relayout('plot-yz',{'xaxis.range':rYZ[0],'yaxis.range':rYZ[1]});
|
||
|
||
Plotly.restyle('plot-zx',{x:[tz],y:[tx]},1);
|
||
Plotly.restyle('plot-zx',{x:[[pos.z]],y:[[pos.x]]},2);
|
||
const rZX=square2D([0,pos.z].concat(tz),[0,pos.x].concat(tx));
|
||
Plotly.relayout('plot-zx',{'xaxis.range':rZX[0],'yaxis.range':rZX[1]});
|
||
|
||
document.getElementById('status').textContent =
|
||
`Обновлений: ${updateCount} | Traj: ${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():
|
||
conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
|
||
try:
|
||
cur = conn.execute(
|
||
"SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp DESC LIMIT 1"
|
||
)
|
||
row = cur.fetchone()
|
||
if row:
|
||
pos = {'x': row[0], 'y': row[1], 'z': row[2]}
|
||
orient = {'roll': row[3] or 0.0, 'pitch': row[4] or 0.0, 'yaw': row[5] or 0.0}
|
||
else:
|
||
pos = {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||
orient = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
|
||
|
||
cur = conn.execute(
|
||
f"SELECT x, y, z FROM trajectory ORDER BY timestamp ASC 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],
|
||
}
|
||
|
||
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, 'orient': orient, '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() |