updates
This commit is contained in:
Salijoghli
parent
8d75f952e4
commit
ede90d2209
701
add_devices.py
701
add_devices.py
@ -3,9 +3,7 @@ import math
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
# -----------------------
|
||||
# CONFIG
|
||||
# -----------------------
|
||||
SCALE = 0.0254
|
||||
FIXED_Y = 2.4
|
||||
BUTTON_Y = 2.2
|
||||
@ -18,10 +16,7 @@ csv_candidates = list(SCRIPT_DIR.glob("*.csv"))
|
||||
if not csv_candidates:
|
||||
raise RuntimeError("No CSV found in script directory.")
|
||||
if len(csv_candidates) > 1:
|
||||
raise RuntimeError(
|
||||
"Multiple CSV files found. Please keep only ONE CSV in the folder:\n"
|
||||
+ "\n".join(c.name for c in csv_candidates)
|
||||
)
|
||||
raise RuntimeError("Multiple CSV files found. Please keep only ONE CSV in the folder.")
|
||||
CSV_PATH = csv_candidates[0]
|
||||
|
||||
SCENE_DIR = SCRIPT_DIR.parent
|
||||
@ -30,31 +25,19 @@ OUTPUT_DIR.mkdir(exist_ok=True)
|
||||
|
||||
SENSOR_SCENE_PATH = "res://parts/DiffuseSensor.tscn"
|
||||
SENSOR_RES_ID = "auto_sensor"
|
||||
|
||||
# ✅ correct push button scene
|
||||
BUTTON_SCENE_PATH = "res://parts/PushButton.tscn"
|
||||
BUTTON_RES_ID = "auto_button"
|
||||
|
||||
# -----------------------
|
||||
# LOAD CSV DEVICES
|
||||
# -----------------------
|
||||
tpe_devices = []
|
||||
btn_devices = []
|
||||
ss_devices = []
|
||||
epc_devices = []
|
||||
|
||||
# LOAD CSV
|
||||
tpe_devices, btn_devices, ss_devices, epc_devices = [], [], [], []
|
||||
with open(CSV_PATH, newline="", encoding="utf-8") as f:
|
||||
reader = csv.DictReader(f)
|
||||
for row in reader:
|
||||
rt = (row.get("record_type") or "").strip().upper()
|
||||
if rt == "TPE":
|
||||
tpe_devices.append(row)
|
||||
elif rt == "S":
|
||||
btn_devices.append(row)
|
||||
elif rt == "SS":
|
||||
ss_devices.append(row)
|
||||
elif rt == "EPC":
|
||||
epc_devices.append(row)
|
||||
if rt == "TPE": tpe_devices.append(row)
|
||||
elif rt == "S": btn_devices.append(row)
|
||||
elif rt == "SS": ss_devices.append(row)
|
||||
elif rt == "EPC": epc_devices.append(row)
|
||||
|
||||
if not tpe_devices:
|
||||
raise RuntimeError("No TPE records found in CSV.")
|
||||
@ -62,30 +45,24 @@ if not tpe_devices:
|
||||
devices_by_conveyor = {}
|
||||
for d in tpe_devices:
|
||||
key = (d.get("conveyor_key") or "").strip()
|
||||
if key:
|
||||
devices_by_conveyor.setdefault(key, []).append(d)
|
||||
if key: devices_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
btn_by_conveyor = {}
|
||||
for d in btn_devices:
|
||||
key = (d.get("conveyor_key") or "").strip()
|
||||
if key:
|
||||
btn_by_conveyor.setdefault(key, []).append(d)
|
||||
if key: btn_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
ss_by_conveyor = {}
|
||||
for d in ss_devices:
|
||||
key = (d.get("conveyor_key") or "").strip()
|
||||
if key:
|
||||
ss_by_conveyor.setdefault(key, []).append(d)
|
||||
if key: ss_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
epc_by_conveyor = {}
|
||||
for d in epc_devices:
|
||||
key = (d.get("conveyor_key") or "").strip()
|
||||
if key:
|
||||
epc_by_conveyor.setdefault(key, []).append(d)
|
||||
if key: epc_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
# -----------------------
|
||||
# SELECT SCENE
|
||||
# -----------------------
|
||||
scene_files = list(SCENE_DIR.glob("*.tscn"))
|
||||
if not scene_files:
|
||||
raise RuntimeError("No .tscn files found in parent directory.")
|
||||
@ -93,22 +70,18 @@ if not scene_files:
|
||||
print("\nAvailable scenes:")
|
||||
for i, s in enumerate(scene_files, 1):
|
||||
print(f"[{i}] {s.name}")
|
||||
|
||||
choice = int(input("\nSelect scene number: ")) - 1
|
||||
if choice < 0 or choice >= len(scene_files):
|
||||
raise RuntimeError("Invalid selection.")
|
||||
SCENE_PATH = scene_files[choice]
|
||||
|
||||
# -----------------------
|
||||
# DEVICE SELECTION MENU
|
||||
# -----------------------
|
||||
# DEVICE SELECTION
|
||||
print("\nWhat devices do you want to place?")
|
||||
print("[1] All")
|
||||
print("[2] Sensors only")
|
||||
print("[3] Buttons only (S)")
|
||||
print("[4] SS only")
|
||||
print("[5] EPC only")
|
||||
|
||||
device_choice = int(input("\nSelect option: "))
|
||||
if device_choice < 1 or device_choice > 5:
|
||||
raise RuntimeError("Invalid selection.")
|
||||
@ -118,238 +91,232 @@ place_buttons = device_choice in (1, 3)
|
||||
place_ss = device_choice in (1, 4)
|
||||
place_epc = device_choice in (1, 5)
|
||||
|
||||
# -----------------------
|
||||
# READ SCENE
|
||||
# -----------------------
|
||||
scene_text = SCENE_PATH.read_text(encoding="utf-8")
|
||||
lines = scene_text.splitlines()
|
||||
|
||||
# -----------------------
|
||||
# PARSE CONVEYORS
|
||||
# -----------------------
|
||||
conveyors = {}
|
||||
# PARSE EXISTING DEVICES
|
||||
def parse_existing_devices(lines, res_id):
|
||||
"""Extract device names and line indices from scene."""
|
||||
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
|
||||
has_size = False
|
||||
pending_basis = None
|
||||
pending_position = None
|
||||
conveyor_width = CONVEYOR_WIDTH
|
||||
conveyor_size = None
|
||||
# Get existing devices based on what we're placing
|
||||
existing_sensors = parse_existing_devices(lines, SENSOR_RES_ID) if place_sensors else []
|
||||
existing_buttons = parse_existing_devices(lines, BUTTON_RES_ID) if (place_buttons or place_ss or place_epc) else []
|
||||
|
||||
# GENERATE EXPECTED DEVICE NAMES
|
||||
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:
|
||||
if line.startswith("[node name="):
|
||||
if node_name and pending_basis and pending_position:
|
||||
conveyors[node_name] = {
|
||||
"basis": pending_basis,
|
||||
"has_size": has_size,
|
||||
"width": conveyor_width,
|
||||
"position": pending_position,
|
||||
"size": conveyor_size
|
||||
"basis": pending_basis, "has_size": has_size,
|
||||
"width": conveyor_width, "position": pending_position, "size": conveyor_size
|
||||
}
|
||||
|
||||
m = re.search(r'name="([^"]+)"', line)
|
||||
node_name = m.group(1) if m else None
|
||||
has_size = False
|
||||
pending_basis = None
|
||||
pending_position = None
|
||||
conveyor_width = CONVEYOR_WIDTH
|
||||
conveyor_size = None
|
||||
has_size, pending_basis, pending_position = False, None, None
|
||||
conveyor_width, conveyor_size = CONVEYOR_WIDTH, None
|
||||
continue
|
||||
|
||||
|
||||
if node_name and line.strip().startswith("transform = Transform3D"):
|
||||
vals = [float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")]
|
||||
pending_basis = (vals[0], vals[2], vals[6], vals[8])
|
||||
pending_position = (vals[9], vals[10], vals[11])
|
||||
|
||||
|
||||
if node_name and line.strip().startswith("size = Vector3"):
|
||||
has_size = True
|
||||
conveyor_size = tuple(
|
||||
float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(",")
|
||||
)
|
||||
|
||||
conveyor_size = tuple(float(v) for v in re.search(r"\(([^)]+)\)", line).group(1).split(","))
|
||||
|
||||
if node_name and line.strip().startswith("conveyor_width ="):
|
||||
conveyor_width = float(line.split("=")[1])
|
||||
|
||||
if node_name and pending_basis and pending_position:
|
||||
conveyors[node_name] = {
|
||||
"basis": pending_basis,
|
||||
"has_size": has_size,
|
||||
"width": conveyor_width,
|
||||
"position": pending_position,
|
||||
"size": conveyor_size
|
||||
"basis": pending_basis, "has_size": has_size,
|
||||
"width": conveyor_width, "position": pending_position, "size": conveyor_size
|
||||
}
|
||||
|
||||
# -----------------------
|
||||
# ENSURE RESOURCES
|
||||
# -----------------------
|
||||
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"))
|
||||
lines.insert(idx + 1,
|
||||
f'[ext_resource type="PackedScene" path="{SENSOR_SCENE_PATH}" id="{SENSOR_RES_ID}"]'
|
||||
)
|
||||
lines.insert(idx + 1, 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:
|
||||
idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource"))
|
||||
lines.insert(idx + 1,
|
||||
f'[ext_resource type="PackedScene" path="{BUTTON_SCENE_PATH}" id="{BUTTON_RES_ID}"]'
|
||||
)
|
||||
lines.insert(idx + 1, f'[ext_resource type="PackedScene" path="{BUTTON_SCENE_PATH}" id="{BUTTON_RES_ID}"]')
|
||||
|
||||
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
|
||||
# -----------------------
|
||||
def yaw_from_x_axis(xx, xz):
|
||||
return math.atan2(-xz, xx)
|
||||
|
||||
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 yaw_from_x_axis(xx, xz): return math.atan2(-xz, xx)
|
||||
def yaw_from_z_axis(zx, zz): return math.atan2(zx, zz)
|
||||
def godot_yaw_from_cad(cad_deg): return math.radians((270 - cad_deg) % 360)
|
||||
def compute_edge_offset(sensor_yaw, width):
|
||||
side = sensor_yaw - math.pi / 2
|
||||
d = (width / 2) + EDGE_CLEARANCE
|
||||
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):
|
||||
cx, _, cz = info["position"]
|
||||
rx = math.cos(conveyor_yaw - math.pi / 2)
|
||||
rz = math.sin(conveyor_yaw - math.pi / 2)
|
||||
vx = csv_x - cx
|
||||
vz = csv_z - cz
|
||||
rx, rz = math.cos(conveyor_yaw - math.pi / 2), math.sin(conveyor_yaw - math.pi / 2)
|
||||
vx, vz = csv_x - cx, csv_z - cz
|
||||
return vx * rx + vz * rz
|
||||
|
||||
def project_to_conveyor(info, yaw, csv_x, csv_z):
|
||||
cx, _, cz = info["position"]
|
||||
fx = math.cos(yaw)
|
||||
fz = math.sin(yaw)
|
||||
vx = csv_x - cx
|
||||
vz = csv_z - cz
|
||||
fx, fz = math.cos(yaw), math.sin(yaw)
|
||||
vx, vz = csv_x - cx, csv_z - cz
|
||||
t = vx * fx + vz * fz
|
||||
return cx + fx * t, cz + fz * t
|
||||
|
||||
# -----------------------
|
||||
# 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)
|
||||
|
||||
MIN_BUTTON_SPACING = 0.5
|
||||
def check_collision_and_adjust(x, z, yaw, existing_buttons):
|
||||
"""
|
||||
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):
|
||||
for attempt in range(20):
|
||||
collision = False
|
||||
|
||||
for btn in existing_buttons:
|
||||
dist = math.sqrt((x - btn['x'])**2 + (z - btn['z'])**2)
|
||||
if dist < MIN_BUTTON_SPACING:
|
||||
if math.sqrt((x - btn['x'])**2 + (z - btn['z'])**2) < MIN_BUTTON_SPACING:
|
||||
collision = True
|
||||
break
|
||||
|
||||
if not collision:
|
||||
return x, z
|
||||
|
||||
# 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")
|
||||
if not collision: return x, z
|
||||
x += math.cos(yaw) * 0.3
|
||||
z += math.sin(yaw) * 0.3
|
||||
return x, z
|
||||
|
||||
# -----------------------
|
||||
# APPEND DEVICES
|
||||
# -----------------------
|
||||
node_blocks = []
|
||||
node_blocks, skipped_devices, added_devices = [], [], []
|
||||
|
||||
# -------- SENSORS --------
|
||||
# SENSORS
|
||||
if place_sensors:
|
||||
for key in sorted(devices_by_conveyor.keys()):
|
||||
devs = devices_by_conveyor[key]
|
||||
|
||||
if key not in conveyors:
|
||||
print(f"⚠ Conveyor not found: {key}")
|
||||
continue
|
||||
|
||||
info = conveyors[key]
|
||||
|
||||
info = conveyors[key]
|
||||
xx, xz, zx, zz = info["basis"]
|
||||
width = info["width"]
|
||||
|
||||
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
|
||||
|
||||
|
||||
for i, d in enumerate(devs):
|
||||
name = f"{key}_TPE{i+1}"
|
||||
if duplicate_action == "skip" and name in duplicates:
|
||||
skipped_devices.append(name)
|
||||
continue
|
||||
|
||||
tag = f"{name}_OIP"
|
||||
|
||||
max_range = width + BEAM_RANGE_ADJUSTMENT
|
||||
|
||||
x, z = compute_sensor_position(info, yaw, i, len(devs))
|
||||
|
||||
|
||||
LONGITUDINAL_OFFSET = 0.15
|
||||
|
||||
if x is not None:
|
||||
if len(devs) == 1:
|
||||
x -= math.cos(yaw) * LONGITUDINAL_OFFSET
|
||||
@ -362,36 +329,22 @@ if place_sensors:
|
||||
x -= math.cos(yaw) * LONGITUDINAL_OFFSET
|
||||
z -= math.sin(yaw) * LONGITUDINAL_OFFSET
|
||||
else:
|
||||
x = float(d["tpe_x"]) * SCALE
|
||||
z = -float(d["tpe_y"]) * SCALE
|
||||
|
||||
if "tpe_rotation" in d and d["tpe_rotation"] not in ("", None):
|
||||
tpe_rot = float(d["tpe_rotation"])
|
||||
else:
|
||||
tpe_rot = 0.0
|
||||
|
||||
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)
|
||||
x, z = float(d["tpe_x"]) * SCALE, -float(d["tpe_y"]) * SCALE
|
||||
|
||||
tpe_rot = float(d.get("tpe_rotation") or 0)
|
||||
blk_rot = float(d.get("tpe_block_rotation") or 0)
|
||||
|
||||
if tpe_rot != 0 or blk_rot != 0:
|
||||
sensor_yaw = godot_yaw_from_cad((blk_rot + tpe_rot) % 360)
|
||||
else:
|
||||
sensor_yaw = yaw + math.pi
|
||||
|
||||
|
||||
ox, oz = compute_edge_offset(sensor_yaw, width)
|
||||
x += ox
|
||||
z += oz
|
||||
|
||||
x, z = x + ox, z + oz
|
||||
c, s = math.cos(sensor_yaw), math.sin(sensor_yaw)
|
||||
|
||||
transform = (
|
||||
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
|
||||
f"{x:.6f},{FIXED_Y:.6f},{z:.6f})"
|
||||
)
|
||||
|
||||
|
||||
transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{x:.6f},{FIXED_Y:.6f},{z:.6f})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{name}" parent="." instance=ExtResource("{SENSOR_RES_ID}")]\n'
|
||||
f"transform = {transform}\n"
|
||||
@ -399,66 +352,43 @@ if place_sensors:
|
||||
f"enable_comms = true\n"
|
||||
f'tag_name = "{tag}"\n'
|
||||
)
|
||||
added_devices.append(name)
|
||||
|
||||
# -------- BUTTONS (S) --------
|
||||
# BUTTONS (S)
|
||||
if place_buttons:
|
||||
for key in sorted(btn_by_conveyor.keys()):
|
||||
devs = btn_by_conveyor[key]
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
if key not in conveyors:
|
||||
print(f"⚠ Conveyor not found for buttons: {key}")
|
||||
continue
|
||||
|
||||
|
||||
info = conveyors[key]
|
||||
xx, xz, zx, zz = info["basis"]
|
||||
width = info["width"]
|
||||
|
||||
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
|
||||
|
||||
|
||||
for i, d in enumerate(devs):
|
||||
name = f"{key}_S{i+1}"
|
||||
|
||||
csv_x = float(d["dev_x"]) * SCALE
|
||||
csv_z = -float(d["dev_y"]) * SCALE
|
||||
|
||||
if duplicate_action == "skip" and name in duplicates:
|
||||
skipped_devices.append(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)
|
||||
side = side_of_conveyor(info, yaw, csv_x, csv_z)
|
||||
|
||||
if abs(side) < 0.05:
|
||||
side_sign = 0.0
|
||||
else:
|
||||
side_sign = 1.0 if side > 0 else -1.0
|
||||
|
||||
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
|
||||
side_sign = 0 if abs(side) < 0.05 else (1 if side > 0 else -1)
|
||||
|
||||
px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
|
||||
d_off = (width / 2) + 0.1
|
||||
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
|
||||
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
|
||||
|
||||
|
||||
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},"
|
||||
f"{x:.6f},{BUTTON_Y:.6f},{z:.6f})"
|
||||
)
|
||||
|
||||
transform = f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},{x:.6f},{BUTTON_Y:.6f},{z:.6f})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {transform}\n"
|
||||
@ -468,106 +398,65 @@ if place_buttons:
|
||||
f'pushbutton_tag_name = "{name}_PB_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})
|
||||
added_devices.append(name)
|
||||
|
||||
# -------- SS STATIONS --------
|
||||
# SS STATIONS
|
||||
if place_ss:
|
||||
for key in sorted(ss_by_conveyor.keys()):
|
||||
devs = ss_by_conveyor[key]
|
||||
|
||||
# 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)
|
||||
|
||||
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)
|
||||
|
||||
if key not in conveyors:
|
||||
print(f"⚠ Conveyor not found for SS station: {key}")
|
||||
continue
|
||||
|
||||
|
||||
info = conveyors[key]
|
||||
xx, xz, zx, zz = info["basis"]
|
||||
width = info["width"]
|
||||
|
||||
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
|
||||
|
||||
|
||||
for i, d in enumerate(devs):
|
||||
station = f"{key}_SS{i+1}"
|
||||
|
||||
csv_x = float(d["dev_x"]) * SCALE
|
||||
csv_z = -float(d["dev_y"]) * SCALE
|
||||
|
||||
# Get conveyor center position
|
||||
start_node, stop_node = f"{station}_SPB", f"{station}_STPB"
|
||||
|
||||
if duplicate_action == "skip" and (start_node in duplicates or stop_node in duplicates):
|
||||
if start_node in duplicates: skipped_devices.append(start_node)
|
||||
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"]
|
||||
|
||||
# Check if SS is to the left of conveyor (lower X coordinate)
|
||||
if csv_x < cx:
|
||||
# Place at START edge CENTER of conveyor, facing opposite to flow
|
||||
if not info["size"]:
|
||||
x = cx
|
||||
z = cz
|
||||
x, z = cx, cz
|
||||
else:
|
||||
length = info["size"][0]
|
||||
# Position at start edge center (negative direction along conveyor)
|
||||
x = cx - math.cos(yaw) * (length / 2)
|
||||
z = cz - math.sin(yaw) * (length / 2)
|
||||
|
||||
# Rotation: 270 degrees (-90), perpendicular to flow, pointing left
|
||||
# Calculate perpendicular based on conveyor's actual basis vectors
|
||||
# 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)
|
||||
perp_yaw = math.atan2(zx, zz)
|
||||
base_yaw = perp_yaw + math.pi / 2
|
||||
fx, fz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
|
||||
STATION_SPREAD = 0.45
|
||||
else:
|
||||
# Original logic: project to conveyor and place on side
|
||||
x, z = project_to_conveyor(info, yaw, csv_x, csv_z)
|
||||
side = side_of_conveyor(info, yaw, csv_x, csv_z)
|
||||
|
||||
if abs(side) < 0.05:
|
||||
side_sign = 0.0
|
||||
else:
|
||||
side_sign = 1.0 if side > 0 else -1.0
|
||||
|
||||
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)
|
||||
side_sign = 0 if abs(side) < 0.05 else (1 if side > 0 else -1)
|
||||
px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
|
||||
d_off = (width / 2) + 0.1
|
||||
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
|
||||
fx, fz = math.cos(yaw), math.sin(yaw)
|
||||
STATION_SPREAD = 0.45
|
||||
|
||||
# Check for collisions with existing buttons
|
||||
|
||||
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
|
||||
|
||||
# -------- START (SPB) --------
|
||||
start_node = f"{station}_SPB"
|
||||
start_x = x + fx * (STATION_SPREAD / 2)
|
||||
start_z = z + fz * (STATION_SPREAD / 2)
|
||||
|
||||
|
||||
# START
|
||||
start_x, start_z = x + fx * (STATION_SPREAD / 2), z + fz * (STATION_SPREAD / 2)
|
||||
c, s = math.cos(base_yaw), math.sin(base_yaw)
|
||||
start_transform = (
|
||||
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})"
|
||||
)
|
||||
|
||||
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})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{start_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {start_transform}\n"
|
||||
@ -577,20 +466,13 @@ if place_ss:
|
||||
f'pushbutton_tag_name = "{station}_SPB_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})
|
||||
|
||||
# -------- STOP (STPB) --------
|
||||
stop_node = f"{station}_STPB"
|
||||
stop_x = x - fx * (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},"
|
||||
f"{stop_x:.6f},{BUTTON_Y:.6f},{stop_z:.6f})"
|
||||
)
|
||||
|
||||
added_devices.append(start_node)
|
||||
|
||||
# STOP
|
||||
stop_x, stop_z = x - fx * (STATION_SPREAD / 2), 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})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{stop_node}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {stop_transform}\n"
|
||||
@ -600,70 +482,50 @@ if place_ss:
|
||||
f"enable_comms = true\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})
|
||||
added_devices.append(stop_node)
|
||||
|
||||
# -------- EPC DEVICES --------
|
||||
# EPC DEVICES
|
||||
if place_epc:
|
||||
for key in sorted(epc_by_conveyor.keys()):
|
||||
devs = epc_by_conveyor[key]
|
||||
|
||||
if key not in conveyors:
|
||||
print(f"⚠ Conveyor not found for EPC: {key}")
|
||||
continue
|
||||
|
||||
|
||||
info = conveyors[key]
|
||||
xx, xz, zx, zz = info["basis"]
|
||||
width = info["width"]
|
||||
|
||||
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
|
||||
|
||||
|
||||
for i, d in enumerate(devs):
|
||||
name = d.get("dev_name") or f"{key}_EPC{i+1}"
|
||||
|
||||
csv_x = float(d["dev_x"]) * SCALE
|
||||
csv_z = -float(d["dev_y"]) * SCALE
|
||||
|
||||
# Same side logic as S buttons
|
||||
ch1_name, ch2_name = f"{name}_CH1", f"{name}_CH2"
|
||||
|
||||
if duplicate_action == "skip" and (ch1_name in duplicates or ch2_name in duplicates):
|
||||
if ch1_name in duplicates: skipped_devices.append(ch1_name)
|
||||
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)
|
||||
side = side_of_conveyor(info, yaw, csv_x, csv_z)
|
||||
|
||||
side_sign = 1.0 if side >= 0 else -1.0
|
||||
|
||||
px = math.cos(yaw - math.pi / 2)
|
||||
pz = math.sin(yaw - math.pi / 2)
|
||||
|
||||
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)
|
||||
side_sign = 1 if side >= 0 else -1
|
||||
|
||||
px, pz = math.cos(yaw - math.pi / 2), math.sin(yaw - math.pi / 2)
|
||||
d_off = (width / 2) + 0.1
|
||||
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_SPREAD = 0.35
|
||||
fx = math.cos(yaw)
|
||||
fz = math.sin(yaw)
|
||||
|
||||
# Check for collisions with existing buttons
|
||||
fx, fz = math.cos(yaw), math.sin(yaw)
|
||||
x, z = check_collision_and_adjust(x, z, yaw, existing_buttons)
|
||||
|
||||
c, s = math.cos(base_yaw), math.sin(base_yaw)
|
||||
|
||||
# -------- CH1 --------
|
||||
ch1_name = f"{name}_CH1"
|
||||
ch1_x = x + fx * (EPC_SPREAD / 2)
|
||||
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})"
|
||||
)
|
||||
|
||||
|
||||
# CH1
|
||||
ch1_x, ch1_z = x + fx * (EPC_SPREAD / 2), z + fz * (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})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{ch1_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {ch1_transform}\n"
|
||||
@ -673,40 +535,53 @@ if place_epc:
|
||||
f"enable_comms = true\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})
|
||||
|
||||
# -------- CH2 --------
|
||||
ch2_name = f"{name}_CH2"
|
||||
ch2_x = x - fx * (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},"
|
||||
f"{ch2_x:.6f},{BUTTON_Y:.6f},{ch2_z:.6f})"
|
||||
)
|
||||
|
||||
added_devices.append(ch1_name)
|
||||
|
||||
# CH2
|
||||
ch2_x, ch2_z = x - fx * (EPC_SPREAD / 2), 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})"
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{ch2_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {ch2_transform}\n"
|
||||
f'text = "EPC"\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'pushbutton_tag_name = "{name}_CH2_OIP"\n'
|
||||
)
|
||||
|
||||
# Add to existing buttons list
|
||||
existing_buttons.append({'name': ch2_name, 'x': ch2_x, 'z': ch2_z})
|
||||
|
||||
added_devices.append(ch2_name)
|
||||
|
||||
scene_text += "".join(node_blocks)
|
||||
|
||||
# -----------------------
|
||||
# WRITE OUTPUT
|
||||
# -----------------------
|
||||
out_path = OUTPUT_DIR / f"{SCENE_PATH.stem}_devices{SCENE_PATH.suffix}"
|
||||
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}")
|
||||
Loading…
x
Reference in New Issue
Block a user