guga.kakhadze/simulation-generation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

updates

Browse Source
This commit is contained in:
Salijoghli 2026-01-20 08:09:56 +04:00
parent 8d75f952e4
commit ede90d2209
1 changed files with 288 additions and 413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

701
add_devices.py
View File

@ -3,9 +3,7 @@ import math
from pathlib import Path from pathlib import Path
import re import re
# -----------------------
# CONFIG # CONFIG
# -----------------------
SCALE = 0.0254 SCALE = 0.0254
FIXED_Y = 2.4 FIXED_Y = 2.4
BUTTON_Y = 2.2 BUTTON_Y = 2.2
@ -18,10 +16,7 @@ csv_candidates = list(SCRIPT_DIR.glob("*.csv"))
if not csv_candidates: if not csv_candidates:
raise RuntimeError("No CSV found in script directory.") raise RuntimeError("No CSV found in script directory.")
if len(csv_candidates) > 1: if len(csv_candidates) > 1:
raise RuntimeError( raise RuntimeError("Multiple CSV files found. Please keep only ONE CSV in the folder.")
"Multiple CSV files found. Please keep only ONE CSV in the folder:\n"
+ "\n".join(c.name for c in csv_candidates)
)
CSV_PATH = csv_candidates[0] CSV_PATH = csv_candidates[0]
SCENE_DIR = SCRIPT_DIR.parent SCENE_DIR = SCRIPT_DIR.parent
@ -30,31 +25,19 @@ OUTPUT_DIR.mkdir(exist_ok=True)
SENSOR_SCENE_PATH = "res://parts/DiffuseSensor.tscn" SENSOR_SCENE_PATH = "res://parts/DiffuseSensor.tscn"
SENSOR_RES_ID = "auto_sensor" SENSOR_RES_ID = "auto_sensor"
# ✅ correct push button scene
BUTTON_SCENE_PATH = "res://parts/PushButton.tscn" BUTTON_SCENE_PATH = "res://parts/PushButton.tscn"
BUTTON_RES_ID = "auto_button" BUTTON_RES_ID = "auto_button"
# ----------------------- # LOAD CSV
# LOAD CSV DEVICES tpe_devices, btn_devices, ss_devices, epc_devices = [], [], [], []
# -----------------------
tpe_devices = []
btn_devices = []
ss_devices = []
epc_devices = []
with open(CSV_PATH, newline="", encoding="utf-8") as f: with open(CSV_PATH, newline="", encoding="utf-8") as f:
reader = csv.DictReader(f) reader = csv.DictReader(f)
for row in reader: for row in reader:
rt = (row.get("record_type") or "").strip().upper() rt = (row.get("record_type") or "").strip().upper()
if rt == "TPE": if rt == "TPE": tpe_devices.append(row)
tpe_devices.append(row) elif rt == "S": btn_devices.append(row)
elif rt == "S": elif rt == "SS": ss_devices.append(row)
btn_devices.append(row) elif rt == "EPC": epc_devices.append(row)
elif rt == "SS":
ss_devices.append(row)
elif rt == "EPC":
epc_devices.append(row)
if not tpe_devices: if not tpe_devices:
raise RuntimeError("No TPE records found in CSV.") raise RuntimeError("No TPE records found in CSV.")
@ -62,30 +45,24 @@ if not tpe_devices:
devices_by_conveyor = {} devices_by_conveyor = {}
for d in tpe_devices: for d in tpe_devices:
key = (d.get("conveyor_key") or "").strip() key = (d.get("conveyor_key") or "").strip()
if key: if key: devices_by_conveyor.setdefault(key, []).append(d)
devices_by_conveyor.setdefault(key, []).append(d)
btn_by_conveyor = {} btn_by_conveyor = {}
for d in btn_devices: for d in btn_devices:
key = (d.get("conveyor_key") or "").strip() key = (d.get("conveyor_key") or "").strip()
if key: if key: btn_by_conveyor.setdefault(key, []).append(d)
btn_by_conveyor.setdefault(key, []).append(d)
ss_by_conveyor = {} ss_by_conveyor = {}
for d in ss_devices: for d in ss_devices:
key = (d.get("conveyor_key") or "").strip() key = (d.get("conveyor_key") or "").strip()
if key: if key: ss_by_conveyor.setdefault(key, []).append(d)
ss_by_conveyor.setdefault(key, []).append(d)
epc_by_conveyor = {} epc_by_conveyor = {}
for d in epc_devices: for d in epc_devices:
key = (d.get("conveyor_key") or "").strip() key = (d.get("conveyor_key") or "").strip()
if key: if key: epc_by_conveyor.setdefault(key, []).append(d)
epc_by_conveyor.setdefault(key, []).append(d)
# -----------------------
# SELECT SCENE # SELECT SCENE
# -----------------------
scene_files = list(SCENE_DIR.glob("*.tscn")) scene_files = list(SCENE_DIR.glob("*.tscn"))
if not scene_files: if not scene_files:
raise RuntimeError("No .tscn files found in parent directory.") raise RuntimeError("No .tscn files found in parent directory.")
@ -93,22 +70,18 @@ if not scene_files:
print("\nAvailable scenes:") print("\nAvailable scenes:")
for i, s in enumerate(scene_files, 1): for i, s in enumerate(scene_files, 1):
print(f"[{i}] {s.name}") print(f"[{i}] {s.name}")
choice = int(input("\nSelect scene number: ")) - 1 choice = int(input("\nSelect scene number: ")) - 1
if choice < 0 or choice >= len(scene_files): if choice < 0 or choice >= len(scene_files):
raise RuntimeError("Invalid selection.") raise RuntimeError("Invalid selection.")
SCENE_PATH = scene_files[choice] SCENE_PATH = scene_files[choice]
# ----------------------- # DEVICE SELECTION
# DEVICE SELECTION MENU
# -----------------------
print("\nWhat devices do you want to place?") print("\nWhat devices do you want to place?")
print("[1] All") print("[1] All")
print("[2] Sensors only") print("[2] Sensors only")
print("[3] Buttons only (S)") print("[3] Buttons only (S)")
print("[4] SS only") print("[4] SS only")
print("[5] EPC only") print("[5] EPC only")
device_choice = int(input("\nSelect option: ")) device_choice = int(input("\nSelect option: "))
if device_choice < 1 or device_choice > 5: if device_choice < 1 or device_choice > 5:
raise RuntimeError("Invalid selection.") raise RuntimeError("Invalid selection.")
@ -118,238 +91,232 @@ place_buttons = device_choice in (1, 3)
place_ss = device_choice in (1, 4) place_ss = device_choice in (1, 4)
place_epc = device_choice in (1, 5) place_epc = device_choice in (1, 5)
# -----------------------
# READ SCENE # READ SCENE
# -----------------------
scene_text = SCENE_PATH.read_text(encoding="utf-8") scene_text = SCENE_PATH.read_text(encoding="utf-8")
lines = scene_text.splitlines() lines = scene_text.splitlines()
# ----------------------- # PARSE EXISTING DEVICES
# PARSE CONVEYORS def parse_existing_devices(lines, res_id):
# ----------------------- """Extract device names and line indices from scene."""
conveyors = {} existing = []
for i, line in enumerate(lines):
if f'instance=ExtResource("{res_id}")' in line and '[node name="' in line:
m = re.search(r'name="([^"]+)"', line)
if m:
existing.append({'name': m.group(1), 'line_idx': i})
return existing
node_name = None # Get existing devices based on what we're placing
has_size = False existing_sensors = parse_existing_devices(lines, SENSOR_RES_ID) if place_sensors else []
pending_basis = None existing_buttons = parse_existing_devices(lines, BUTTON_RES_ID) if (place_buttons or place_ss or place_epc) else []
pending_position = None
conveyor_width = CONVEYOR_WIDTH # GENERATE EXPECTED DEVICE NAMES
conveyor_size = None expected_names = []
if place_sensors:
for key in sorted(devices_by_conveyor.keys()):
for i in range(len(devices_by_conveyor[key])):
expected_names.append(f"{key}_TPE{i+1}")
if place_buttons:
for key in sorted(btn_by_conveyor.keys()):
for i in range(len(btn_by_conveyor[key])):
expected_names.append(f"{key}_S{i+1}")
if place_ss:
for key in sorted(ss_by_conveyor.keys()):
for i in range(len(ss_by_conveyor[key])):
station = f"{key}_SS{i+1}"
expected_names.extend([f"{station}_SPB", f"{station}_STPB"])
if place_epc:
for key in sorted(epc_by_conveyor.keys()):
for i, d in enumerate(epc_by_conveyor[key]):
name = d.get("dev_name") or f"{key}_EPC{i+1}"
expected_names.extend([f"{name}_CH1", f"{name}_CH2"])
# FIND DUPLICATES
existing_names = [d['name'] for d in existing_sensors] + [d['name'] for d in existing_buttons]
duplicates = [name for name in expected_names if name in existing_names]
# HANDLE DUPLICATES
duplicate_action = None
if duplicates:
print(f"\n⚠ Found {len(duplicates)} duplicate device(s):")
for dup in duplicates[:10]:
print(f" - {dup}")
if len(duplicates) > 10:
print(f" ... and {len(duplicates) - 10} more")
print("\nHow do you want to handle duplicates?")
print("[1] Skip duplicates (keep existing, only add new)")
print("[2] Replace all (delete old, add new)")
print("[3] Cancel operation")
dup_choice = int(input("\nSelect option: "))
if dup_choice == 3:
print("Operation cancelled.")
exit(0)
elif dup_choice == 1:
duplicate_action = "skip"
elif dup_choice == 2:
duplicate_action = "replace"
else:
raise RuntimeError("Invalid selection.")
# REMOVE OLD DEVICES IF REPLACING
if duplicate_action == "replace":
nodes_to_remove = [d for d in (existing_sensors + existing_buttons) if d['name'] in duplicates]
lines_to_remove = set()
for node in nodes_to_remove:
start_idx = node['line_idx']
end_idx = start_idx + 1
while end_idx < len(lines):
if lines[end_idx].startswith('[node') or lines[end_idx].startswith('[connection'):
break
end_idx += 1
for idx in range(start_idx, end_idx):
lines_to_remove.add(idx)
lines = [line for i, line in enumerate(lines) if i not in lines_to_remove]
scene_text = "\n".join(lines) + "\n"
print(f"✔ Removed {len(nodes_to_remove)} old device node(s)")
# PARSE CONVEYORS
conveyors = {}
node_name, has_size, pending_basis, pending_position = None, False, None, None
conveyor_width, conveyor_size = CONVEYOR_WIDTH, None
for line in lines: for line in lines:
if line.startswith("[node name="): if line.startswith("[node name="):
if node_name and pending_basis and pending_position: if node_name and pending_basis and pending_position:
conveyors[node_name] = { conveyors[node_name] = {
"basis": pending_basis, "basis": pending_basis, "has_size": has_size,
"has_size": has_size, "width": conveyor_width, "position": pending_position, "size": conveyor_size
"width": conveyor_width,
"position": pending_position,
"size": conveyor_size
} }
m = re.search(r'name="([^"]+)"', line) m = re.search(r'name="([^"]+)"', line)
node_name = m.group(1) if m else None node_name = m.group(1) if m else None
has_size = False has_size, pending_basis, pending_position = False, None, None
pending_basis = None conveyor_width, conveyor_size = CONVEYOR_WIDTH, None
pending_position = None
conveyor_width = CONVEYOR_WIDTH
conveyor_size = None
continue continue
if node_name and line.strip().startswith("transform = Transform3D"): if node_name and line.strip().startswith("transform = Transform3D"):
vals = [float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")] vals = [float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")]
pending_basis = (vals[0], vals[2], vals[6], vals[8]) pending_basis = (vals[0], vals[2], vals[6], vals[8])
pending_position = (vals[9], vals[10], vals[11]) pending_position = (vals[9], vals[10], vals[11])
if node_name and line.strip().startswith("size = Vector3"): if node_name and line.strip().startswith("size = Vector3"):
has_size = True has_size = True
conveyor_size = tuple( conveyor_size = tuple(float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(","))
float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")
)
if node_name and line.strip().startswith("conveyor_width ="): if node_name and line.strip().startswith("conveyor_width ="):
conveyor_width = float(line.split("=")[1]) conveyor_width = float(line.split("=")[1])
if node_name and pending_basis and pending_position: if node_name and pending_basis and pending_position:
conveyors[node_name] = { conveyors[node_name] = {
"basis": pending_basis, "basis": pending_basis, "has_size": has_size,
"has_size": has_size, "width": conveyor_width, "position": pending_position, "size": conveyor_size
"width": conveyor_width,
"position": pending_position,
"size": conveyor_size
} }
# -----------------------
# ENSURE RESOURCES # ENSURE RESOURCES
# -----------------------
if place_sensors and SENSOR_SCENE_PATH not in scene_text: if place_sensors and SENSOR_SCENE_PATH not in scene_text:
idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource")) idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource"))
lines.insert(idx + 1, lines.insert(idx + 1, f'[ext_resource type="PackedScene" path="{SENSOR_SCENE_PATH}" id="{SENSOR_RES_ID}"]')
f'[ext_resource type="PackedScene" path="{SENSOR_SCENE_PATH}" id="{SENSOR_RES_ID}"]'
)
if (place_buttons or place_ss or place_epc) and BUTTON_SCENE_PATH not in scene_text: if (place_buttons or place_ss or place_epc) and BUTTON_SCENE_PATH not in scene_text:
idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource")) idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource"))
lines.insert(idx + 1, lines.insert(idx + 1, f'[ext_resource type="PackedScene" path="{BUTTON_SCENE_PATH}" id="{BUTTON_RES_ID}"]')
f'[ext_resource type="PackedScene" path="{BUTTON_SCENE_PATH}" id="{BUTTON_RES_ID}"]'
)
scene_text = "\n".join(lines) + "\n" scene_text = "\n".join(lines) + "\n"
# ----------------------- # PARSE EXISTING BUTTONS FOR COLLISION
existing_buttons = []
in_button_node, current_button_name, current_button_transform = False, None, None
for line in lines:
if f'instance=ExtResource("{BUTTON_RES_ID}")' in line:
in_button_node = True
m = re.search(r'name="([^"]+)"', line)
if m: current_button_name = m.group(1)
if in_button_node and line.strip().startswith("transform = Transform3D"):
vals = [float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")]
current_button_transform = (vals[9], vals[11])
if current_button_name and current_button_transform:
existing_buttons.append({'name': current_button_name, 'x': current_button_transform[0], 'z': current_button_transform[1]})
in_button_node, current_button_name, current_button_transform = False, None, None
# HELPERS # HELPERS
# ----------------------- def yaw_from_x_axis(xx, xz): return math.atan2(-xz, xx)
def yaw_from_x_axis(xx, xz): def yaw_from_z_axis(zx, zz): return math.atan2(zx, zz)
return math.atan2(-xz, xx) def godot_yaw_from_cad(cad_deg): return math.radians((270 - cad_deg) % 360)
def yaw_from_z_axis(zx, zz):
return math.atan2(zx, zz)
def compute_sensor_position(info, yaw, idx, total):
px, _, pz = info["position"]
if not info["size"]:
return None, None
length = info["size"][0]
if total == 1:
off = length / 2
elif total == 2:
off = (-length / 2) if idx == 0 else (length / 2)
else:
return None, None
return (
px + math.cos(yaw) * off,
pz + math.sin(yaw) * off
)
def godot_yaw_from_cad(cad_deg: float) -> float:
return math.radians((270 - cad_deg) % 360)
def compute_edge_offset(sensor_yaw, width): def compute_edge_offset(sensor_yaw, width):
side = sensor_yaw - math.pi / 2 side = sensor_yaw - math.pi / 2
d = (width / 2) + EDGE_CLEARANCE d = (width / 2) + EDGE_CLEARANCE
return math.cos(side) * d, math.sin(side) * d return math.cos(side) * d, math.sin(side) * d
# --- BUTTON HELPERS (UNCHANGED) --- def compute_sensor_position(info, yaw, idx, total):
px, _, pz = info["position"]
if not info["size"]: return None, None
length = info["size"][0]
if total == 1: off = length / 2
elif total == 2: off = (-length / 2) if idx == 0 else (length / 2)
else: return None, None
return (px + math.cos(yaw) * off, pz + math.sin(yaw) * off)
def side_of_conveyor(info, conveyor_yaw, csv_x, csv_z): def side_of_conveyor(info, conveyor_yaw, csv_x, csv_z):
cx, _, cz = info["position"] cx, _, cz = info["position"]
rx = math.cos(conveyor_yaw - math.pi / 2) rx, rz = math.cos(conveyor_yaw - math.pi / 2), math.sin(conveyor_yaw - math.pi / 2)
rz = math.sin(conveyor_yaw - math.pi / 2) vx, vz = csv_x - cx, csv_z - cz
vx = csv_x - cx
vz = csv_z - cz
return vx * rx + vz * rz return vx * rx + vz * rz
def project_to_conveyor(info, yaw, csv_x, csv_z): def project_to_conveyor(info, yaw, csv_x, csv_z):
cx, _, cz = info["position"] cx, _, cz = info["position"]
fx = math.cos(yaw) fx, fz = math.cos(yaw), math.sin(yaw)
fz = math.sin(yaw) vx, vz = csv_x - cx, csv_z - cz
vx = csv_x - cx
vz = csv_z - cz
t = vx * fx + vz * fz t = vx * fx + vz * fz
return cx + fx * t, cz + fz * t return cx + fx * t, cz + fz * t
# ----------------------- MIN_BUTTON_SPACING = 0.5
# PARSE EXISTING BUTTONS FROM SCENE
# -----------------------
existing_buttons = []
in_button_node = False
current_button_name = None
current_button_transform = None
for line in lines:
# Detect button nodes
if f'instance=ExtResource("{BUTTON_RES_ID}")' in line:
in_button_node = True
m = re.search(r'name="([^"]+)"', line)
if m:
current_button_name = m.group(1)
# Extract transform if we're in a button node
if in_button_node and line.strip().startswith("transform = Transform3D"):
vals = [float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")]
current_button_transform = (vals[9], vals[11]) # x, z position
if current_button_name and current_button_transform:
existing_buttons.append({
'name': current_button_name,
'x': current_button_transform[0],
'z': current_button_transform[1]
})
in_button_node = False
current_button_name = None
current_button_transform = None
print(f"\n📍 Found {len(existing_buttons)} existing buttons in scene")
# -----------------------
# COLLISION DETECTION HELPER
# -----------------------
MIN_BUTTON_SPACING = 0.5 # minimum distance between buttons (meters)
def check_collision_and_adjust(x, z, yaw, existing_buttons): def check_collision_and_adjust(x, z, yaw, existing_buttons):
""" for attempt in range(20):
Check if position (x, z) collides with existing buttons.
If collision detected, shift along conveyor direction (yaw).
Returns adjusted (x, z) position.
"""
max_attempts = 20
shift_distance = 0.3 # how much to shift each attempt
for attempt in range(max_attempts):
collision = False collision = False
for btn in existing_buttons: for btn in existing_buttons:
dist = math.sqrt((x - btn['x'])**2 + (z - btn['z'])**2) if math.sqrt((x - btn['x'])**2 + (z - btn['z'])**2) < MIN_BUTTON_SPACING:
if dist < MIN_BUTTON_SPACING:
collision = True collision = True
break break
if not collision: return x, z
if not collision: x += math.cos(yaw) * 0.3
return x, z z += math.sin(yaw) * 0.3
# Shift forward along conveyor
x += math.cos(yaw) * shift_distance
z += math.sin(yaw) * shift_distance
print(f"⚠ Warning: Could not find collision-free position after {max_attempts} attempts")
return x, z return x, z
# -----------------------
# APPEND DEVICES # APPEND DEVICES
# ----------------------- node_blocks, skipped_devices, added_devices = [], [], []
node_blocks = []
# -------- SENSORS -------- # SENSORS
if place_sensors: if place_sensors:
for key in sorted(devices_by_conveyor.keys()): for key in sorted(devices_by_conveyor.keys()):
devs = devices_by_conveyor[key] devs = devices_by_conveyor[key]
if key not in conveyors: if key not in conveyors:
print(f"⚠ Conveyor not found: {key}") print(f"⚠ Conveyor not found: {key}")
continue continue
info = conveyors[key]
info = conveyors[key]
xx, xz, zx, zz = info["basis"] xx, xz, zx, zz = info["basis"]
width = info["width"] width = info["width"]
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz) yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
for i, d in enumerate(devs): for i, d in enumerate(devs):
name = f"{key}_TPE{i+1}" name = f"{key}_TPE{i+1}"
if duplicate_action == "skip" and name in duplicates:
skipped_devices.append(name)
continue
tag = f"{name}_OIP" tag = f"{name}_OIP"
max_range = width + BEAM_RANGE_ADJUSTMENT max_range = width + BEAM_RANGE_ADJUSTMENT
x, z = compute_sensor_position(info, yaw, i, len(devs)) x, z = compute_sensor_position(info, yaw, i, len(devs))
LONGITUDINAL_OFFSET = 0.15 LONGITUDINAL_OFFSET = 0.15
if x is not None: if x is not None:
if len(devs) == 1: if len(devs) == 1:
x -= math.cos(yaw) * LONGITUDINAL_OFFSET x -= math.cos(yaw) * LONGITUDINAL_OFFSET
@ -362,36 +329,22 @@ if place_sensors:
x -= math.cos(yaw) * LONGITUDINAL_OFFSET x -= math.cos(yaw) * LONGITUDINAL_OFFSET
z -= math.sin(yaw) * LONGITUDINAL_OFFSET z -= math.sin(yaw) * LONGITUDINAL_OFFSET
else: else:
x = float(d["tpe_x"]) * SCALE x, z = float(d["tpe_x"]) * SCALE, -float(d["tpe_y"]) * SCALE
z = -float(d["tpe_y"]) * SCALE
tpe_rot = float(d.get("tpe_rotation") or 0)
if "tpe_rotation" in d and d["tpe_rotation"] not in ("", None): blk_rot = float(d.get("tpe_block_rotation") or 0)
tpe_rot = float(d["tpe_rotation"])
else: if tpe_rot != 0 or blk_rot != 0:
tpe_rot = 0.0 sensor_yaw = godot_yaw_from_cad((blk_rot + tpe_rot) % 360)
if "tpe_block_rotation" in d and d["tpe_block_rotation"] not in ("", None):
blk_rot = float(d["tpe_block_rotation"])
else:
blk_rot = 0.0
if tpe_rot != 0.0 or blk_rot != 0.0:
cad_world_rot = (blk_rot + tpe_rot) % 360.0
sensor_yaw = godot_yaw_from_cad(cad_world_rot)
else: else:
sensor_yaw = yaw + math.pi sensor_yaw = yaw + math.pi
ox, oz = compute_edge_offset(sensor_yaw, width) ox, oz = compute_edge_offset(sensor_yaw, width)
x += ox x, z = x + ox, z + oz
z += oz
c, s = math.cos(sensor_yaw), math.sin(sensor_yaw) c, s = math.cos(sensor_yaw), math.sin(sensor_yaw)
transform = ( transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{x:.6f},{FIXED_Y:.6f},{z:.6f})"
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{x:.6f},{FIXED_Y:.6f},{z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{name}" parent="." instance=ExtResource("{SENSOR_RES_ID}")]\n' f'\n[node name="{name}" parent="." instance=ExtResource("{SENSOR_RES_ID}")]\n'
f"transform = {transform}\n" f"transform = {transform}\n"
@ -399,66 +352,43 @@ if place_sensors:
f"enable_comms = true\n" f"enable_comms = true\n"
f'tag_name = "{tag}"\n' f'tag_name = "{tag}"\n'
) )
added_devices.append(name)
# -------- BUTTONS (S) -------- # BUTTONS (S)
if place_buttons: if place_buttons:
for key in sorted(btn_by_conveyor.keys()): for key in sorted(btn_by_conveyor.keys()):
devs = btn_by_conveyor[key] devs = btn_by_conveyor[key]
devs = sorted(devs, key=lambda d: int(re.search(r"_S(\d+)$", d.get("dev_name") or "").group(1)) if re.search(r"_S(\d+)$", d.get("dev_name") or "") else 999)
def btn_index(d):
name = (d.get("dev_name") or "")
m = re.search(r"_S(\d+)$", name)
return int(m.group(1)) if m else 999
devs = sorted(devs, key=btn_index)
if key not in conveyors: if key not in conveyors:
print(f"⚠ Conveyor not found for buttons: {key}") print(f"⚠ Conveyor not found for buttons: {key}")
continue continue
info = conveyors[key] info = conveyors[key]
xx, xz, zx, zz = info["basis"] xx, xz, zx, zz = info["basis"]
width = info["width"] width = info["width"]
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz) yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
for i, d in enumerate(devs): for i, d in enumerate(devs):
name = f"{key}_S{i+1}" name = f"{key}_S{i+1}"
if duplicate_action == "skip" and name in duplicates:
csv_x = float(d["dev_x"]) * SCALE skipped_devices.append(name)
csv_z = -float(d["dev_y"]) * SCALE continue
csv_x, csv_z = float(d["dev_x"]) * SCALE, -float(d["dev_y"]) * SCALE
x, z = project_to_conveyor(info, yaw, csv_x, csv_z) x, z = project_to_conveyor(info, yaw, csv_x, csv_z)
side = side_of_conveyor(info, yaw, csv_x, csv_z) side = side_of_conveyor(info, yaw, csv_x, csv_z)
side_sign = 0 if abs(side) < 0.05 else (1 if side > 0 else -1)
if abs(side) < 0.05:
side_sign = 0.0 px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
else: d_off = (width / 2) + 0.1
side_sign = 1.0 if side > 0 else -1.0 x, z = x + px * d_off * side_sign, z + pz * d_off * side_sign
btn_yaw = yaw + math.pi if side_sign >= 0 else yaw
px = math.cos(yaw - math.pi / 2)
pz = math.sin(yaw - math.pi / 2)
BUTTON_CLEARANCE = 0.1
d_off = (width / 2) + BUTTON_CLEARANCE
x += px * d_off * side_sign
z += pz * d_off * side_sign
if side_sign >= 0:
btn_yaw = yaw + math.pi
else:
btn_yaw = yaw
# Check for collisions with existing buttons
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons) x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
c, s = math.cos(btn_yaw), math.sin(btn_yaw) c, s = math.cos(btn_yaw), math.sin(btn_yaw)
transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{x:.6f},{BUTTON_Y:.6f},{z:.6f})"
transform = (
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{x:.6f},{BUTTON_Y:.6f},{z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n' f'\n[node name="{name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
f"transform = {transform}\n" f"transform = {transform}\n"
@ -468,106 +398,65 @@ if place_buttons:
f'pushbutton_tag_name = "{name}_PB_OIP"\n' f'pushbutton_tag_name = "{name}_PB_OIP"\n'
f'lamp_tag_name = "{name}_LT_OIP"\n' f'lamp_tag_name = "{name}_LT_OIP"\n'
) )
# Add to existing buttons list to prevent overlap with other new buttons
existing_buttons.append({'name': name, 'x': x, 'z': z}) existing_buttons.append({'name': name, 'x': x, 'z': z})
added_devices.append(name)
# -------- SS STATIONS -------- # SS STATIONS
if place_ss: if place_ss:
for key in sorted(ss_by_conveyor.keys()): for key in sorted(ss_by_conveyor.keys()):
devs = ss_by_conveyor[key] devs = ss_by_conveyor[key]
devs = sorted(devs, key=lambda d: int(re.search(r"_SS(\d+)$", d.get("dev_name") or "").group(1)) if re.search(r"_SS(\d+)$", d.get("dev_name") or "") else 999)
# sort SS1, SS2...
def ss_index(d):
name = (d.get("dev_name") or "")
m = re.search(r"_SS(\d+)$", name)
return int(m.group(1)) if m else 999
devs = sorted(devs, key=ss_index)
if key not in conveyors: if key not in conveyors:
print(f"⚠ Conveyor not found for SS station: {key}") print(f"⚠ Conveyor not found for SS station: {key}")
continue continue
info = conveyors[key] info = conveyors[key]
xx, xz, zx, zz = info["basis"] xx, xz, zx, zz = info["basis"]
width = info["width"] width = info["width"]
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz) yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
for i, d in enumerate(devs): for i, d in enumerate(devs):
station = f"{key}_SS{i+1}" station = f"{key}_SS{i+1}"
start_node, stop_node = f"{station}_SPB", f"{station}_STPB"
csv_x = float(d["dev_x"]) * SCALE
csv_z = -float(d["dev_y"]) * SCALE if duplicate_action == "skip" and (start_node in duplicates or stop_node in duplicates):
if start_node in duplicates: skipped_devices.append(start_node)
# Get conveyor center position if stop_node in duplicates: skipped_devices.append(stop_node)
continue
csv_x, csv_z = float(d["dev_x"]) * SCALE, -float(d["dev_y"]) * SCALE
cx, _, cz = info["position"] cx, _, cz = info["position"]
# Check if SS is to the left of conveyor (lower X coordinate)
if csv_x < cx: if csv_x < cx:
# Place at START edge CENTER of conveyor, facing opposite to flow
if not info["size"]: if not info["size"]:
x = cx x, z = cx, cz
z = cz
else: else:
length = info["size"][0] length = info["size"][0]
# Position at start edge center (negative direction along conveyor)
x = cx - math.cos(yaw) * (length / 2) x = cx - math.cos(yaw) * (length / 2)
z = cz - math.sin(yaw) * (length / 2) z = cz - math.sin(yaw) * (length / 2)
perp_yaw = math.atan2(zx, zz)
# Rotation: 270 degrees (-90), perpendicular to flow, pointing left base_yaw = perp_yaw + math.pi / 2
# Calculate perpendicular based on conveyor's actual basis vectors fx, fz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
# Right perpendicular uses the conveyor's Z-axis direction
perp_yaw = math.atan2(zx, zz) # perpendicular right
base_yaw = perp_yaw + math.pi / 2 # rotate 90° to get 270° (-90°)
# Spread buttons along the perpendicular (left-right across conveyor width)
fx = math.cos(yaw - math.pi / 2) # perpendicular to conveyor flow
fz = math.sin(yaw - math.pi / 2)
STATION_SPREAD = 0.45 STATION_SPREAD = 0.45
else: else:
# Original logic: project to conveyor and place on side
x, z = project_to_conveyor(info, yaw, csv_x, csv_z) x, z = project_to_conveyor(info, yaw, csv_x, csv_z)
side = side_of_conveyor(info, yaw, csv_x, csv_z) side = side_of_conveyor(info, yaw, csv_x, csv_z)
side_sign = 0 if abs(side) < 0.05 else (1 if side > 0 else -1)
if abs(side) < 0.05: px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
side_sign = 0.0 d_off = (width / 2) + 0.1
else: x, z = x + px * d_off * side_sign, z + pz * d_off * side_sign
side_sign = 1.0 if side > 0 else -1.0 base_yaw = yaw + math.pi if side_sign >= 0 else yaw
fx, fz = math.cos(yaw), math.sin(yaw)
px = math.cos(yaw - math.pi / 2)
pz = math.sin(yaw - math.pi / 2)
BUTTON_CLEARANCE = 0.1
d_off = (width / 2) + BUTTON_CLEARANCE
x += px * d_off * side_sign
z += pz * d_off * side_sign
if side_sign >= 0:
base_yaw = yaw + math.pi
else:
base_yaw = yaw
# Spread buttons along conveyor direction
fx = math.cos(yaw)
fz = math.sin(yaw)
STATION_SPREAD = 0.45 STATION_SPREAD = 0.45
# Check for collisions with existing buttons
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons) x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
# -------- START (SPB) -------- # START
start_node = f"{station}_SPB" start_x, start_z = x + fx * (STATION_SPREAD / 2), z + fz * (STATION_SPREAD / 2)
start_x = x + fx * (STATION_SPREAD / 2)
start_z = z + fz * (STATION_SPREAD / 2)
c, s = math.cos(base_yaw), math.sin(base_yaw) c, s = math.cos(base_yaw), math.sin(base_yaw)
start_transform = ( start_transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{start_x:.6f},{BUTTON_Y:.6f},{start_z:.6f})"
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{start_x:.6f},{BUTTON_Y:.6f},{start_z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{start_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n' f'\n[node name="{start_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
f"transform = {start_transform}\n" f"transform = {start_transform}\n"
@ -577,20 +466,13 @@ if place_ss:
f'pushbutton_tag_name = "{station}_SPB_OIP"\n' f'pushbutton_tag_name = "{station}_SPB_OIP"\n'
f'lamp_tag_name = "{station}_SPB_LT_OIP"\n' f'lamp_tag_name = "{station}_SPB_LT_OIP"\n'
) )
# Add to existing buttons list
existing_buttons.append({'name': start_node, 'x': start_x, 'z': start_z}) existing_buttons.append({'name': start_node, 'x': start_x, 'z': start_z})
added_devices.append(start_node)
# -------- STOP (STPB) --------
stop_node = f"{station}_STPB" # STOP
stop_x = x - fx * (STATION_SPREAD / 2) stop_x, stop_z = x - fx * (STATION_SPREAD / 2), z - fz * (STATION_SPREAD / 2)
stop_z = z - fz * (STATION_SPREAD / 2) stop_transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{stop_x:.6f},{BUTTON_Y:.6f},{stop_z:.6f})"
stop_transform = (
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{stop_x:.6f},{BUTTON_Y:.6f},{stop_z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{stop_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n' f'\n[node name="{stop_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
f"transform = {stop_transform}\n" f"transform = {stop_transform}\n"
@ -600,70 +482,50 @@ if place_ss:
f"enable_comms = true\n" f"enable_comms = true\n"
f'pushbutton_tag_name = "{station}_STPB_OIP"\n' f'pushbutton_tag_name = "{station}_STPB_OIP"\n'
) )
# Add to existing buttons list
existing_buttons.append({'name': stop_node, 'x': stop_x, 'z': stop_z}) existing_buttons.append({'name': stop_node, 'x': stop_x, 'z': stop_z})
added_devices.append(stop_node)
# -------- EPC DEVICES -------- # EPC DEVICES
if place_epc: if place_epc:
for key in sorted(epc_by_conveyor.keys()): for key in sorted(epc_by_conveyor.keys()):
devs = epc_by_conveyor[key] devs = epc_by_conveyor[key]
if key not in conveyors: if key not in conveyors:
print(f"⚠ Conveyor not found for EPC: {key}") print(f"⚠ Conveyor not found for EPC: {key}")
continue continue
info = conveyors[key] info = conveyors[key]
xx, xz, zx, zz = info["basis"] xx, xz, zx, zz = info["basis"]
width = info["width"] width = info["width"]
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz) yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
for i, d in enumerate(devs): for i, d in enumerate(devs):
name = d.get("dev_name") or f"{key}_EPC{i+1}" name = d.get("dev_name") or f"{key}_EPC{i+1}"
ch1_name, ch2_name = f"{name}_CH1", f"{name}_CH2"
csv_x = float(d["dev_x"]) * SCALE
csv_z = -float(d["dev_y"]) * SCALE if duplicate_action == "skip" and (ch1_name in duplicates or ch2_name in duplicates):
if ch1_name in duplicates: skipped_devices.append(ch1_name)
# Same side logic as S buttons if ch2_name in duplicates: skipped_devices.append(ch2_name)
continue
csv_x, csv_z = float(d["dev_x"]) * SCALE, -float(d["dev_y"]) * SCALE
x, z = project_to_conveyor(info, yaw, csv_x, csv_z) x, z = project_to_conveyor(info, yaw, csv_x, csv_z)
side = side_of_conveyor(info, yaw, csv_x, csv_z) side = side_of_conveyor(info, yaw, csv_x, csv_z)
side_sign = 1 if side >= 0 else -1
side_sign = 1.0 if side >= 0 else -1.0
px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
px = math.cos(yaw - math.pi / 2) d_off = (width / 2) + 0.1
pz = math.sin(yaw - math.pi / 2) x, z = x + px * d_off * side_sign, z + pz * d_off * side_sign
base_yaw = yaw + math.pi if side_sign >= 0 else yaw
EPC_CLEARANCE = 0.1
d_off = (width / 2) + EPC_CLEARANCE
x += px * d_off * side_sign
z += pz * d_off * side_sign
if side_sign >= 0:
base_yaw = yaw + math.pi
else:
base_yaw = yaw
# Spread along conveyor direction (like SS buttons do)
EPC_SPREAD = 0.35 EPC_SPREAD = 0.35
fx = math.cos(yaw) fx, fz = math.cos(yaw), math.sin(yaw)
fz = math.sin(yaw)
# Check for collisions with existing buttons
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons) x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
c, s = math.cos(base_yaw), math.sin(base_yaw) c, s = math.cos(base_yaw), math.sin(base_yaw)
# -------- CH1 -------- # CH1
ch1_name = f"{name}_CH1" ch1_x, ch1_z = x + fx * (EPC_SPREAD / 2), z + fz * (EPC_SPREAD / 2)
ch1_x = x + fx * (EPC_SPREAD / 2) ch1_transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{ch1_x:.6f},{BUTTON_Y:.6f},{ch1_z:.6f})"
ch1_z = z + fz * (EPC_SPREAD / 2)
ch1_transform = (
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{ch1_x:.6f},{BUTTON_Y:.6f},{ch1_z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{ch1_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n' f'\n[node name="{ch1_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
f"transform = {ch1_transform}\n" f"transform = {ch1_transform}\n"
@ -673,40 +535,53 @@ if place_epc:
f"enable_comms = true\n" f"enable_comms = true\n"
f'pushbutton_tag_name = "{name}_CH1_OIP"\n' f'pushbutton_tag_name = "{name}_CH1_OIP"\n'
) )
# Add to existing buttons list
existing_buttons.append({'name': ch1_name, 'x': ch1_x, 'z': ch1_z}) existing_buttons.append({'name': ch1_name, 'x': ch1_x, 'z': ch1_z})
added_devices.append(ch1_name)
# -------- CH2 --------
ch2_name = f"{name}_CH2" # CH2
ch2_x = x - fx * (EPC_SPREAD / 2) ch2_x, ch2_z = x - fx * (EPC_SPREAD / 2), z - fz * (EPC_SPREAD / 2)
ch2_z = z - fz * (EPC_SPREAD / 2) ch2_transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{ch2_x:.6f},{BUTTON_Y:.6f},{ch2_z:.6f})"
ch2_transform = (
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
f"{ch2_x:.6f},{BUTTON_Y:.6f},{ch2_z:.6f})"
)
node_blocks.append( node_blocks.append(
f'\n[node name="{ch2_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n' f'\n[node name="{ch2_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
f"transform = {ch2_transform}\n" f"transform = {ch2_transform}\n"
f'text = "EPC"\n' f'text = "EPC"\n'
f"button_color = Color(1, 0.15, 0.15, 1)\n" f"button_color = Color(1, 0.15, 0.15, 1)\n"
f"normally_closed = true\n" f"normally_closed =true\n"
f"enable_comms = true\n" f"enable_comms = true\n"
f'pushbutton_tag_name = "{name}_CH2_OIP"\n' f'pushbutton_tag_name = "{name}_CH2_OIP"\n'
) )
# Add to existing buttons list
existing_buttons.append({'name': ch2_name, 'x': ch2_x, 'z': ch2_z}) existing_buttons.append({'name': ch2_name, 'x': ch2_x, 'z': ch2_z})
added_devices.append(ch2_name)
scene_text += "".join(node_blocks) scene_text += "".join(node_blocks)
# -----------------------
# WRITE OUTPUT # WRITE OUTPUT
# -----------------------
out_path = OUTPUT_DIR / f"{SCENE_PATH.stem}_devices{SCENE_PATH.suffix}" out_path = OUTPUT_DIR / f"{SCENE_PATH.stem}_devices{SCENE_PATH.suffix}"
out_path.write_text(scene_text, encoding="utf-8") out_path.write_text(scene_text, encoding="utf-8")
print(f"\n✔ {len(node_blocks)} devices added → {out_path}") # PRINT SUMMARY
print(f"\n{'='*60}")
print("OPERATION SUMMARY")
print(f"{'='*60}")
print(f"✔ Total devices added: {len(added_devices)}")
if skipped_devices:
print(f"⏭ Skipped duplicates: {len(skipped_devices)}")
print(f"\nSkipped devices:")
for dev in skipped_devices[:20]:
print(f" - {dev}")
if len(skipped_devices) > 20:
print(f" ... and {len(skipped_devices) - 20} more")
if duplicate_action == "replace" and duplicates:
print(f"\n🔄 Replaced duplicates: {len(duplicates)}")
print(f"\nReplaced devices:")
for dev in duplicates[:20]:
print(f" - {dev}")
if len(duplicates) > 20:
print(f" ... and {len(duplicates) - 20} more")
print(f"\n{'='*60}")
print(f"Output saved to: {out_path}")
print(f"{'='*60}")