ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Phase 0, 1, 2

Browse Source
This commit is contained in:
chk
2026-06-14 16:58:45 +02:00
parent 0cb2bae554
commit dd8de5674d
6 changed files with 773 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
server/homingOrchestrator.js Normal file
View File

@@ -0,0 +1,140 @@
/**
* homingOrchestrator.js
* Vollständiger Homing-Ablauf: Board-Pipeline (1→2→3b) + 4b-Schleife.
*
* Abhängigkeiten werden von server.js per Parameter übergeben
* (kein Circular-Import-Problem).
*/
import path from 'path';
import fs from 'fs';
import fsPromises from 'fs/promises';
/**
* Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen
* (aruco_marker_poses.json). Nutzt den Durchschnitt der x_mm aller
* Nicht-Board-Marker. Fallback: 0.0 wenn keine Arm-Marker sichtbar.
*
* @param {string} arucoJsonPath
* @returns {number} x_mm
*/
export function estimateXFromMarkers(arucoJsonPath) {
try {
const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
const armMarkers = (data.markers ?? []).filter(
m => m.link && m.link !== 'Board',
);
if (armMarkers.length === 0) return 0.0;
const sumX = armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0);
return sumX / armMarkers.length;
} catch {
return 0.0;
}
}
/**
* Führt den vollständigen Homing-Ablauf als SSE-Stream aus.
*
* @param {{
* robotJsonPath: string,
* homingDir: string,
* send: (obj: object) => void,
* runScript: (args: string[], send: Function) => Promise<number>,
* runBoardPipeline: (runDir: string, send: Function) => Promise<void>,
* SCRIPT_4B: string,
* }} opts
*/
export async function runHoming({
robotJsonPath,
homingDir,
send,
runScript,
runBoardPipeline,
SCRIPT_4B,
}) {
// Lauf-Verzeichnis anlegen
const ts = makeTimestamp();
const runDir = path.join(homingDir, ts);
await fsPromises.mkdir(runDir, { recursive: true });
send({ type: 'log', text: `▶ Homing-Run: ${ts}` });
send({ type: 'log', text: `▶ Ordner: ${runDir}` });
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
send({ type: 'log', text: '' });
// ── Schritt 1–3b: Board-Pipeline ─────────────────────────────────────────
send({ type: 'step', step: 1, total: 6, text: 'Foto + Marker-Triangulierung …' });
await runBoardPipeline(runDir, send);
// Prüfen ob aruco_marker_poses.json erzeugt wurde
const arucoJson = path.join(runDir, 'aruco_marker_poses.json');
try {
await fsPromises.access(arucoJson);
} catch {
send({ type: 'error', text: '❌ aruco_marker_poses.json fehlt – Script 3b hat nicht funktioniert.' });
send({ type: 'done', exitCode: -1, runDir: ts });
return;
}
// ── Schritt 2: X-Position bestimmen ─────────────────────────────────────
send({ type: 'step', step: 2, total: 6, text: 'X-Position bestimmen …' });
const xMm = estimateXFromMarkers(arucoJson);
send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` });
send({ type: 'analysis', key: 'x_mm', value: xMm });
// ── Schritt 3–6: 4b-Kette (Arm1 → Ellbow → Arm2 → Hand) ─────────────
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
let fromState = null;
for (let i = 0; i < links.length; i++) {
const link = links[i];
send({ type: 'step', step: 3 + i, total: 6, text: `Gelenkwinkel ${link} …` });
send({ type: 'log', text: `\n─── 4b: ${link} ${'─'.repeat(35 - link.length)}` });
const outputPath = path.join(runDir, `state_${link}.json`);
const args = [
SCRIPT_4B,
'--robot', robotJsonPath,
'--aruco', arucoJson,
'--link', link,
'--output', outputPath,
];
if (fromState) args.push('--from-state', fromState);
else args.push('--x-mm', String(xMm));
const exit = await runScript(args, send);
if (exit !== 0) {
send({ type: 'error', text: `❌ 4b ${link} Exit ${exit}` });
send({ type: 'done', exitCode: exit, runDir: ts });
return;
}
fromState = outputPath;
// Zwischenergebnis an Analysis-Sektion
try {
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
const acc = stateData.accumulated_state ?? stateData;
send({ type: 'analysis', key: `state_${link}`, value: acc });
} catch { /* ignorieren */ }
}
// ── Endergebnis ──────────────────────────────────────────────────────────
try {
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
const finalState = finalData.accumulated_state ?? finalData;
send({ type: 'log', text: '' });
send({ type: 'log', text: `✅ Homing abgeschlossen: ${ts}` });
send({ type: 'done', exitCode: 0, state: finalState, runDir: ts });
} catch (err) {
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
send({ type: 'done', exitCode: -1, runDir: ts });
}
}
/** Timestamp-String YYYYMMDD_HHmmss */
function makeTimestamp() {
const now = new Date();
const p = (n, l = 2) => String(n).padStart(l, '0');
return `${now.getFullYear()}${p(now.getMonth() + 1)}${p(now.getDate())}`
+ `_${p(now.getHours())}${p(now.getMinutes())}${p(now.getSeconds())}`;
}

295
server/server.js
View File

@@ -9,6 +9,7 @@ import process from 'process';
import { spawn } from 'child_process';
import { WebcamClient } from './webcamClient.js';
import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ } from './editRobot.js';
import { runHoming } from './homingOrchestrator.js';
const __filename = fileURLToPath(import.meta.url);
const __dirname = path.dirname(__filename);
@@ -19,8 +20,9 @@ app.use(express.json({ limit: '20mb' }));
const PORT = parseInt(process.env.PORT || process.env.HTTPS_PORT || '2093', 10);
const publicDir = path.join(__dirname, '..', 'public');
const snapshotsDir = path.join(publicDir, 'snapshots');
const WEBCAM_URL = process.env.WEBCAM_URL || '';
const WEBCAM_URL = process.env.WEBCAM_URL || '';
const BODYTRACKER_URL = process.env.BODYTRACKER_URL || '';
const ROBOT_URL = process.env.ROBOT_URL || '';
const HTTPS_KEY_PATH = process.env.HTTPS_KEY_PATH || path.join(__dirname, '..', 'https', 'localhost.key');
const HTTPS_CERT_PATH = process.env.HTTPS_CERT_PATH || path.join(__dirname, '..', 'https', 'localhost.pem');
const HTTPS_PASSPHRASE = process.env.HTTPS_PASSPHRASE || 'abcd';
@@ -430,11 +432,13 @@ app.post('/api/calibration/compute', async (req, res) => {
// ── Board-Erkennung ───────────────────────────────────────────────────────────
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
const homingDataDir = path.join(__dirname, '..', 'data', 'homing');
const ROBOT_JSON = process.env.ROBOT_JSON
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py');
const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py');
const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_poses.py');
const SCRIPT_4B = path.join(__dirname, '..', 'scripts', '4b_revolute_angle.py');
/**
* Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter.
@@ -473,6 +477,103 @@ function runScript(args, send) {
});
}
/**
* Board-Pipeline: Snapshot + Script 1 + Script 2 (pro Kamera) + Script 3b.
* Schreibt Ergebnisse nach runDir (muss bereits existieren).
* Wird von /api/board/run UND /api/homing/run genutzt.
*
* @param {string} runDir – Zielverzeichnis (bereits erstellt)
* @param {Function} send – SSE-Send-Funktion (obj => void)
* @param {{ refSet?: string }} [opts]
*/
async function runBoardPipeline(runDir, send, { refSet } = {}) {
// Kameras ermitteln
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
const cameraIds = (camData.cameras ?? []).map(c => c.id);
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
send({ type: 'log', text: '' });
// Pro Kamera: Foto → Script 1 → Script 2
for (const camId of cameraIds) {
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
// Snapshot
send({ type: 'log', text: 'Foto aufnehmen …' });
let snapResp;
for (let attempt = 1; attempt <= 2; attempt++) {
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
if (snapResp.status !== 503) break;
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
}
if (!snapResp.ok) {
send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` });
continue;
}
const imgPath = path.join(runDir, `${camId}.jpg`);
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
// NPZ suchen – neueste Session, die eine NPZ für diese Kamera enthält
const npzInfo = await findLatestNpzForCamera(camId);
if (!npzInfo) {
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} gefunden – übersprungen` });
continue;
}
const npzPath = npzInfo.npzPath;
send({ type: 'log', text: `▶ NPZ: data/calibration/${npzInfo.session}/${camId}_calibration.npz` });
// Script 1 – ArUco-Erkennung
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
const exit1 = await runScript([
SCRIPT_1,
'-i', imgPath,
'-npz', npzPath,
'-robot', ROBOT_JSON,
'-cameraId', camId,
'-outDir', runDir,
'--saveDebugImage',
], send);
if (exit1 !== 0) {
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
continue;
}
// Script 2 – Kamera-Pose schätzen
const detJson = path.join(runDir, `${camId}_aruco_detection.json`);
try { await fsPromises.access(detJson); }
catch {
send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' });
continue;
}
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', ROBOT_JSON, '-outDir', runDir];
if (refSet) script2Args.push('--refSet', refSet);
const exit2 = await runScript(script2Args, send);
if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
send({ type: 'log', text: '' });
}
// Script 3b: Marker-Triangulierung (benötigt ≥2 Kamera-Posen)
send({ type: 'log', text: '' });
send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' });
const runFiles3b = await fsPromises.readdir(runDir);
const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length;
if (numPoses >= 2) {
send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` });
const exit3b = await runScript([
SCRIPT_3B,
'--evalDir', runDir,
'--robot', ROBOT_JSON,
], send);
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
} else {
send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) – Script 3b braucht ≥2 Kameras` });
}
send({ type: 'log', text: '' });
}
/**
* POST /api/board/run
* 1. Erstellt data/board/{timestamp}/
@@ -513,94 +614,8 @@ app.post('/api/board/run', async (req, res) => {
send({ type: 'log', text: `▶ Referenz-Set: ${refSet ? `"${refSet}" (${refMarkerCount} Marker)` : 'alle'}` });
send({ type: 'log', text: '' });
// 2. Kameras ermitteln
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
const cameraIds = (camData.cameras ?? []).map(c => c.id);
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
send({ type: 'log', text: '' });
// 3. Pro Kamera: Foto → Script 1 → Script 2
for (const camId of cameraIds) {
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
// Snapshot
send({ type: 'log', text: 'Foto aufnehmen …' });
let snapResp;
for (let attempt = 1; attempt <= 2; attempt++) {
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
if (snapResp.status !== 503) break;
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
}
if (!snapResp.ok) {
send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` });
continue;
}
const imgPath = path.join(runDir, `${camId}.jpg`);
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
// NPZ suchen – neueste Session, die eine NPZ für diese Kamera enthält
const npzInfo = await findLatestNpzForCamera(camId);
if (!npzInfo) {
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} gefunden (in keiner Kalibrierungs-Session) – übersprungen` });
continue;
}
const npzPath = npzInfo.npzPath;
send({ type: 'log', text: `▶ NPZ: data/calibration/${npzInfo.session}/${camId}_calibration.npz` });
// Script 1 – ArUco-Erkennung
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
const exit1 = await runScript([
SCRIPT_1,
'-i', imgPath,
'-npz', npzPath,
'-robot', ROBOT_JSON,
'-cameraId', camId,
'-outDir', runDir,
'--saveDebugImage',
], send);
if (exit1 !== 0) {
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
continue;
}
// Script 2 – Kamera-Pose schätzen
const detJson = path.join(runDir, `${camId}_aruco_detection.json`);
try { await fsPromises.access(detJson); }
catch {
send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' });
continue;
}
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', ROBOT_JSON, '-outDir', runDir];
if (refSet) script2Args.push('--refSet', refSet);
const exit2 = await runScript(script2Args, send);
if (exit2 !== 0) {
send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
}
send({ type: 'log', text: '' });
}
// ── Script 3b: Marker-Triangulierung (benötigt ≥2 Kamera-Posen) ──
send({ type: 'log', text: '' });
send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' });
const runFiles3b = await fsPromises.readdir(runDir);
const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length;
if (numPoses >= 2) {
send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` });
const exit3b = await runScript([
SCRIPT_3B,
'--evalDir', runDir,
'--robot', ROBOT_JSON,
], send);
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
} else {
send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) vorhanden – Script 3b braucht ≥2 Kameras für Triangulierung, wird übersprungen.` });
}
send({ type: 'log', text: '' });
// 2–3b: Board-Pipeline (Foto + Scripts 1, 2, 3b)
await runBoardPipeline(runDir, send, { refSet });
send({ type: 'log', text: `✅ Board-Run abgeschlossen: ${ts}` });
send({ type: 'done', exitCode: 0, runDir: ts });
@@ -714,6 +729,108 @@ app.get('/api/board/latest', async (req, res) => {
}
});
// ── Homing ───────────────────────────────────────────────────────────────────
/**
* POST /api/homing/run
* Vollständiger Homing-Ablauf: Board-Pipeline + 4b-Kette (SSE-Stream).
*/
app.post('/api/homing/run', async (req, res) => {
res.setHeader('Content-Type', 'text/event-stream');
res.setHeader('Cache-Control', 'no-cache');
res.setHeader('Connection', 'keep-alive');
res.flushHeaders();
const send = (obj) => {
if (!res.writableEnded) res.write(`data: ${JSON.stringify(obj)}\n\n`);
};
try {
await fsPromises.mkdir(homingDataDir, { recursive: true });
await runHoming({
robotJsonPath: ROBOT_JSON,
homingDir: homingDataDir,
send,
runScript,
runBoardPipeline,
SCRIPT_4B,
});
} catch (err) {
console.error('homing/run error:', err);
try {
send({ type: 'error', text: String(err) });
send({ type: 'done', exitCode: -1 });
} catch {}
}
if (!res.writableEnded) res.end();
});
/**
* POST /api/homing/send-state
* Sendet { state: { x, y, z, a, b, c, e } } an ROBOT_URL/api/state.
*/
app.post('/api/homing/send-state', async (req, res) => {
try {
const { state } = req.body ?? {};
if (!state) return res.status(400).json({ error: '"state" fehlt' });
if (!ROBOT_URL) return res.status(501).json({ error: 'ROBOT_URL ist nicht konfiguriert' });
const url = new URL('/api/state', ROBOT_URL).toString();
const upstream = await fetch(url, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(state),
});
if (!upstream.ok) {
const text = await upstream.text();
return res.status(upstream.status).json({ error: `Robot-Fehler: ${text}` });
}
const result = await upstream.json().catch(() => ({}));
return res.json({ ok: true, result });
} catch (err) {
console.error('homing/send-state error:', err);
return res.status(500).json({ error: String(err) });
}
});
/**
* GET /api/homing/run-data?run=<timestamp>
* Gibt Bilder (base64) und JSON-Dateien eines Homing-Runs zurück.
*/
app.get('/api/homing/run-data', async (req, res) => {
try {
const runName = req.query.run;
if (!runName) return res.status(400).json({ error: '"run" parameter fehlt' });
const runDir = path.join(homingDataDir, runName);
let files = [];
try { files = await fsPromises.readdir(runDir); } catch {}
const images = [];
for (const f of files.sort()) {
if (/\.(jpg|jpeg|png)$/i.test(f)) {
try {
const buf = await fsPromises.readFile(path.join(runDir, f));
images.push({ filename: f, contentBase64: buf.toString('base64'), mimeType: 'image/jpeg' });
} catch {}
}
}
// Letzten accumulated_state zurückgeben
let finalState = null;
const stateFiles = files.filter(f => f.startsWith('state_') && f.endsWith('.json')).sort();
if (stateFiles.length > 0) {
try {
const raw = await fsPromises.readFile(path.join(runDir, stateFiles[stateFiles.length - 1]), 'utf8');
finalState = JSON.parse(raw).accumulated_state ?? null;
} catch {}
}
return res.json({ runDir: runName, images, finalState });
} catch (err) {
return res.status(500).json({ error: String(err) });
}
});
// ── Robot-JSON bearbeiten ─────────────────────────────────────────────────────
/**