robot.json neue Marker einfügen

server/editRobot.js

@@ -23,31 +23,32 @@ async function writeRobot(robotPath, data) {
  * Weist allen Markern, deren z-Position (mm) zwischen zMin und zMax liegt,
  * das angegebene Set und/oder den angegebenen Link zu.
  *
- * - set  (optional): Setzt den set-Wert des Markers.
- * - link (optional): Verschiebt den Marker in diesen Link (wird ggf. angelegt).
+ * - set          (optional): Setzt den set-Wert des Markers.
+ * - link         (optional): Verschiebt den Marker in diesen Link (wird ggf. angelegt).
+ * - extraMarkers (optional): Triangulierte Marker aus aruco_marker_poses.json –
+ *                            werden als neue Einträge in robot.json hinzugefügt,
+ *                            wenn sie noch nicht vorhanden sind.
  *
  * Gibt { numChanged, changes[] } zurück.
- * changes[]: { markerId, oldLink, newLink, oldSet, newSet }
+ * changes[]: { markerId, action, oldLink, newLink, oldSet, newSet }
+ *   action: 'updated' | 'added'
  */
-export async function assignByZRange(robotPath, { zMin, zMax, set, link }) {
+export async function assignByZRange(robotPath, { zMin, zMax, set, link, extraMarkers = [] }) {
   const robot = await readRobot(robotPath);
   const links = robot.links ?? {};
   const changes = [];
 
-  // Snapshot aller Marker (mit aktuellem Link-Name) – vor der Mutation
+  const zLo = Number(zMin);
+  const zHi = Number(zMax);
+
+  // ── Teil 1: Bestehende robot.json-Marker aktualisieren / verschieben ──────────
   const snapshot = [];
   for (const [linkName, linkData] of Object.entries(links)) {
     for (const marker of (linkData.markers ?? [])) {
-      if (Array.isArray(marker.position)) {
-        snapshot.push({ id: marker.id, currentLink: linkName });
-      }
+      if (Array.isArray(marker.position)) snapshot.push({ id: marker.id, currentLink: linkName });
     }
   }
 
-  // Nur Marker im Z-Fenster bearbeiten
-  const zLo = Number(zMin);
-  const zHi = Number(zMax);
-
   for (const { id, currentLink } of snapshot) {
     const srcLinkData = links[currentLink];
     const idx = (srcLinkData?.markers ?? []).findIndex(m => Number(m.id) === id);
@@ -58,6 +59,7 @@ export async function assignByZRange(robotPath, { zMin, zMax, set, link }) {
     if (z < zLo || z > zHi) continue;
 
     const change = {
+      action:  'updated',
       markerId: marker.id,
       oldLink:  currentLink,
       oldSet:   marker.set ?? '',
@@ -65,20 +67,12 @@ export async function assignByZRange(robotPath, { zMin, zMax, set, link }) {
       newSet:   marker.set ?? '',
     };
 
-    // Set setzen
-    if (set !== undefined && set !== '') {
-      marker.set    = set;
-      change.newSet = set;
-    }
+    if (set !== undefined && set !== '') { marker.set = set; change.newSet = set; }
 
-    // Link wechseln
     if (link && link !== currentLink) {
-      // Aus altem Link entfernen
       srcLinkData.markers.splice(idx, 1);
-
-      // Ziel-Link anlegen falls noch nicht vorhanden
-      if (!links[link])          links[link]          = { markers: [] };
-      if (!links[link].markers)  links[link].markers  = [];
+      if (!links[link])         links[link]         = { markers: [] };
+      if (!links[link].markers) links[link].markers = [];
       links[link].markers.push(marker);
       change.newLink = link;
     }
@@ -86,6 +80,45 @@ export async function assignByZRange(robotPath, { zMin, zMax, set, link }) {
     changes.push(change);
   }
 
+  // ── Teil 2: Neue Marker aus 3b-Triangulation einfügen (noch nicht in robot.json) ──
+  // Diese haben keine Position in robot.json – wir übernehmen die gemessene Position.
+  if (link && extraMarkers.length > 0) {
+    const knownIds = new Set();
+    for (const ld of Object.values(links)) {
+      for (const m of (ld.markers ?? [])) knownIds.add(Number(m.id));
+    }
+
+    for (const em of extraMarkers) {
+      const emId  = Number(em.marker_id);
+      const emPos = em.position_mm;           // [x_mm, y_mm, z_mm] robot-Koordinaten
+      if (knownIds.has(emId)) continue;       // bereits in robot.json (evtl. gerade hinzugefügt)
+      if (!Array.isArray(emPos) || emPos.length < 3) continue;
+
+      const z = Number(emPos[2]);
+      if (z < zLo || z > zHi) continue;
+
+      const newMarker = {
+        id:       emId,
+        position: emPos.map(v => Math.round(Number(v) * 100) / 100),  // 2 Dezimalstellen
+      };
+      if (set) newMarker.set = set;
+
+      if (!links[link])         links[link]         = { markers: [] };
+      if (!links[link].markers) links[link].markers = [];
+      links[link].markers.push(newMarker);
+      knownIds.add(emId);
+
+      changes.push({
+        action:   'added',
+        markerId: emId,
+        oldLink:  null,
+        oldSet:   '',
+        newLink:  link,
+        newSet:   set ?? '',
+      });
+    }
+  }
+
   if (changes.length > 0) {
     robot.links = links;
     await writeRobot(robotPath, robot);