141 lines
5.2 KiB
JavaScript
141 lines
5.2 KiB
JavaScript
/**
|
||
* 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())}`;
|
||
}
|