appRobotHoming/server/homingOrchestrator.js

/**
 * 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 });
  }
}

/** 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())}`;
}