diff --git a/server/homingOrchestrator.js b/server/homingOrchestrator.js index 6fe9577..5c7ede2 100644 --- a/server/homingOrchestrator.js +++ b/server/homingOrchestrator.js @@ -42,6 +42,7 @@ export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) { * runScript: (args: string[], send: Function) => Promise, * runBoardPipeline: (runDir: string, send: Function) => Promise, * 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 { - const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8')); - const finalState = finalData.accumulated_state ?? finalData; + 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 }); diff --git a/server/server.js b/server/server.js index 9ebc491..b5b884b 100755 --- a/server/server.js +++ b/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);