Wenn Hand abbricht > Weiter

server/homingOrchestrator.js

@@ -42,6 +42,7 @@ export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
  *   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({
@@ -51,6 +52,7 @@ export async function runHoming({
   runScript,
   runBoardPipeline,
   SCRIPT_4B,
+  SCRIPT_5POSE,
 }) {
   // Lauf-Verzeichnis anlegen
   const ts = makeTimestamp();
@@ -85,6 +87,7 @@ export async function runHoming({
   // ── 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];
@@ -104,9 +107,9 @@ export async function runHoming({
 
     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;
+      send({ type: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
+      chainComplete = false;
+      break;
     }
     fromState = outputPath;
 
@@ -120,8 +123,25 @@ export async function runHoming({
 
   // ── Endergebnis ──────────────────────────────────────────────────────────
   try {
+    let finalState;
+    if (chainComplete) {
       const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
-    const finalState = finalData.accumulated_state ?? finalData;
+      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 });

server/server.js

@@ -445,6 +445,7 @@ const SCRIPT_1      = path.join(__dirname, '..', 'scripts', '1_detect_aruco_obse
 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');
+const SCRIPT_5POSE  = path.join(__dirname, '..', 'scripts', '5_pose_estimation.py');
 
 /**
  * Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter.
@@ -776,6 +777,7 @@ app.post('/api/homing/run', async (req, res) => {
       runScript,
       runBoardPipeline,
       SCRIPT_4B,
+      SCRIPT_5POSE,
     });
   } catch (err) {
     console.error('homing/run error:', err);