263 lines
11 KiB
JavaScript
263 lines
11 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';
|
||
import { estimateXFromParsed } from './homingXEstimate.cjs';
|
||
|
||
/**
|
||
* Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen.
|
||
* Dünner I/O-Wrapper: liest die Dateien und delegiert die reine Geometrie an
|
||
* `estimateXFromParsed()` (`server/homingXEstimate.cjs`, unit-getestet).
|
||
*
|
||
* @param {string} arucoJsonPath Pfad zu aruco_marker_poses.json
|
||
* @param {string} [robotJsonPath] Pfad zu robot.json (für den Gelenk-Offset)
|
||
* @returns {number} x_mm
|
||
*/
|
||
export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
|
||
try {
|
||
const arucoData = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
|
||
const links = robotJsonPath
|
||
? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {})
|
||
: {};
|
||
return estimateXFromParsed(arucoData, links);
|
||
} 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,
|
||
* SCRIPT_5POSE: string,
|
||
* }} opts
|
||
*/
|
||
export async function runHoming({
|
||
robotJsonPath,
|
||
homingDir,
|
||
send,
|
||
runScript,
|
||
runBoardPipeline,
|
||
SCRIPT_4B,
|
||
SCRIPT_5POSE,
|
||
}) {
|
||
// 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, robotJsonPath);
|
||
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;
|
||
let chainComplete = true;
|
||
|
||
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: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
|
||
chainComplete = false;
|
||
break;
|
||
}
|
||
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 {
|
||
let finalState;
|
||
if (chainComplete) {
|
||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||
finalState = finalData.accumulated_state ?? finalData;
|
||
} else {
|
||
// 4b vorzeitig abgebrochen -> 5_pose_estimation.py mit dem letzten
|
||
// erfolgreichen Zwischenstand (falls vorhanden) als Startwert.
|
||
send({ type: 'step', step: 6, total: 6, text: '5_pose_estimation.py (Fallback) …' });
|
||
const poseOut = path.join(runDir, 'robot_state.json');
|
||
const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut];
|
||
if (fromState) args.push('--from-state', fromState);
|
||
const exit = await runScript(args, send);
|
||
if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`);
|
||
const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8'));
|
||
finalState = Object.fromEntries(
|
||
Object.entries(poseData.movements).map(([k, v]) => [k, v.value])
|
||
);
|
||
send({ type: 'analysis', key: 'robot_state', value: poseData });
|
||
}
|
||
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 });
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Führt den Homing-Ablauf offline aus (Bilder und NPZ bereits im runDir).
|
||
* Identische 4b-Kette wie runHoming — ohne Webcam-Zugriff und ohne SSE-Stream.
|
||
* send() akkumuliert Logs; done-Event trägt den finalen State.
|
||
*
|
||
* @param {{
|
||
* robotJsonPath: string,
|
||
* runDir: string,
|
||
* send: (obj: object) => void,
|
||
* runScript: (args: string[], send: Function) => Promise<number>,
|
||
* runBoardPipelineOffline:(runDir: string, send: Function) => Promise<void>,
|
||
* SCRIPT_4B: string,
|
||
* SCRIPT_5POSE: string,
|
||
* }} opts
|
||
*/
|
||
export async function runHomingOffline({
|
||
robotJsonPath,
|
||
runDir,
|
||
send,
|
||
runScript,
|
||
runBoardPipelineOffline,
|
||
SCRIPT_4B,
|
||
SCRIPT_5POSE,
|
||
}) {
|
||
send({ type: 'log', text: `▶ Homing-Offline: ${path.basename(runDir)}` });
|
||
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
|
||
send({ type: 'log', text: '' });
|
||
|
||
// ── Schritt 1: Marker-Triangulierung (Bilder liegen bereits im runDir) ──────
|
||
send({ type: 'step', step: 1, total: 5, text: 'Marker-Triangulierung …' });
|
||
await runBoardPipelineOffline(runDir, send);
|
||
|
||
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 });
|
||
return;
|
||
}
|
||
|
||
// ── Schritt 2: X-Position schätzen ──────────────────────────────────────────
|
||
const xMm = estimateXFromMarkers(arucoJson, robotJsonPath);
|
||
send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` });
|
||
send({ type: 'analysis', key: 'x_mm', value: xMm });
|
||
|
||
// ── Schritte 3–5 (2–4): 4b-Kette Arm1 → Ellbow → Arm2 → Hand ───────────────
|
||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||
let fromState = null;
|
||
let chainComplete = true;
|
||
|
||
for (let i = 0; i < links.length; i++) {
|
||
const link = links[i];
|
||
send({ type: 'step', step: 2 + i, total: 5, 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: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
|
||
chainComplete = false;
|
||
break;
|
||
}
|
||
fromState = outputPath;
|
||
|
||
try {
|
||
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
|
||
send({ type: 'analysis', key: `state_${link}`, value: stateData.accumulated_state ?? stateData });
|
||
} catch { /* ignorieren */ }
|
||
}
|
||
|
||
// ── Endergebnis ──────────────────────────────────────────────────────────────
|
||
try {
|
||
let finalState;
|
||
if (chainComplete) {
|
||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||
finalState = finalData.accumulated_state ?? finalData;
|
||
} else {
|
||
send({ type: 'step', step: 5, total: 5, text: '5_pose_estimation.py (Fallback) …' });
|
||
const poseOut = path.join(runDir, 'robot_state.json');
|
||
const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut];
|
||
if (fromState) args.push('--from-state', fromState);
|
||
const exit = await runScript(args, send);
|
||
if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`);
|
||
const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8'));
|
||
finalState = Object.fromEntries(
|
||
Object.entries(poseData.movements).map(([k, v]) => [k, v.value])
|
||
);
|
||
}
|
||
send({ type: 'log', text: '' });
|
||
send({ type: 'log', text: '✅ Homing-Offline abgeschlossen' });
|
||
send({ type: 'done', exitCode: 0, state: finalState });
|
||
} catch (err) {
|
||
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
|
||
send({ type: 'done', exitCode: -1 });
|
||
}
|
||
}
|
||
|
||
/** 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())}`;
|
||
}
|