392 lines
12 KiB
Python
392 lines
12 KiB
Python
import csv
|
|
import math
|
|
from pathlib import Path
|
|
import re
|
|
|
|
# -----------------------
|
|
# CONFIG
|
|
# -----------------------
|
|
SCALE = 0.0254
|
|
FIXED_Y = 2.4
|
|
BUTTON_Y = 2.3
|
|
CONVEYOR_WIDTH = 1.524
|
|
EDGE_CLEARANCE = 0.45
|
|
BEAM_RANGE_ADJUSTMENT = 0.25
|
|
|
|
SCRIPT_DIR = Path(__file__).resolve().parent
|
|
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)
|
|
)
|
|
CSV_PATH = csv_candidates[0]
|
|
|
|
SCENE_DIR = SCRIPT_DIR.parent
|
|
OUTPUT_DIR = SCRIPT_DIR / "with_devices"
|
|
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 = []
|
|
|
|
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)
|
|
|
|
if not tpe_devices:
|
|
raise RuntimeError("No TPE records found in CSV.")
|
|
|
|
devices_by_conveyor = {}
|
|
for d in tpe_devices:
|
|
key = (d.get("conveyor_key") or "").strip()
|
|
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)
|
|
|
|
# -----------------------
|
|
# SELECT SCENE
|
|
# -----------------------
|
|
scene_files = list(SCENE_DIR.glob("*.tscn"))
|
|
if not scene_files:
|
|
raise RuntimeError("No .tscn files found in parent directory.")
|
|
|
|
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
|
|
# -----------------------
|
|
print("\nWhat devices do you want to place?")
|
|
print("[1] All")
|
|
print("[2] Sensors only")
|
|
print("[3] Buttons only")
|
|
|
|
device_choice = int(input("\nSelect option: "))
|
|
if device_choice < 1 or device_choice > 3:
|
|
raise RuntimeError("Invalid selection.")
|
|
|
|
place_sensors = device_choice in (1, 2)
|
|
place_buttons = device_choice in (1, 3)
|
|
|
|
# -----------------------
|
|
# READ SCENE
|
|
# -----------------------
|
|
scene_text = SCENE_PATH.read_text(encoding="utf-8")
|
|
lines = scene_text.splitlines()
|
|
|
|
# -----------------------
|
|
# PARSE CONVEYORS
|
|
# -----------------------
|
|
conveyors = {}
|
|
|
|
node_name = None
|
|
has_size = False
|
|
pending_basis = None
|
|
pending_position = None
|
|
conveyor_width = CONVEYOR_WIDTH
|
|
conveyor_size = 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
|
|
}
|
|
|
|
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
|
|
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(",")
|
|
)
|
|
|
|
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
|
|
}
|
|
|
|
# -----------------------
|
|
# 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}"]'
|
|
)
|
|
|
|
if place_buttons 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}"]'
|
|
)
|
|
|
|
scene_text = "\n".join(lines) + "\n"
|
|
|
|
# -----------------------
|
|
# 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 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 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
|
|
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
|
|
t = vx * fx + vz * fz
|
|
return cx + fx * t, cz + fz * t
|
|
|
|
# -----------------------
|
|
# APPEND DEVICES
|
|
# -----------------------
|
|
node_blocks = []
|
|
|
|
# -------- 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]
|
|
|
|
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}"
|
|
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
|
|
z -= math.sin(yaw) * LONGITUDINAL_OFFSET
|
|
elif len(devs) == 2:
|
|
if i == 0:
|
|
x += math.cos(yaw) * LONGITUDINAL_OFFSET
|
|
z += math.sin(yaw) * LONGITUDINAL_OFFSET
|
|
else:
|
|
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)
|
|
else:
|
|
sensor_yaw = yaw + math.pi
|
|
|
|
ox, oz = compute_edge_offset(sensor_yaw, width)
|
|
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})"
|
|
)
|
|
|
|
node_blocks.append(
|
|
f'\n[node name="{name}" parent="." instance=ExtResource("{SENSOR_RES_ID}")]\n'
|
|
f"transform = {transform}\n"
|
|
f"max_range = {max_range:.3f}\n"
|
|
f"enable_comms = true\n"
|
|
f'tag_name = "{tag}"\n'
|
|
)
|
|
|
|
# -------- BUTTONS --------
|
|
if place_buttons:
|
|
for key in sorted(btn_by_conveyor.keys()):
|
|
devs = btn_by_conveyor[key]
|
|
|
|
# ✅ force S1, S2
|
|
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:
|
|
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}"
|
|
base_tag = d.get("dev_name", name)
|
|
|
|
csv_x = float(d["dev_x"]) * SCALE
|
|
csv_z = -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)
|
|
|
|
d_off = (width / 2) + EDGE_CLEARANCE
|
|
BUTTON_CLEARANCE = 0.1
|
|
d_off = (width / 2) + BUTTON_CLEARANCE
|
|
x += px * d_off * side_sign
|
|
z += pz * d_off * side_sign
|
|
|
|
# Buttons face along the conveyor, flipped version
|
|
if side_sign >= 0:
|
|
btn_yaw = yaw + math.pi
|
|
else:
|
|
btn_yaw = 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},"
|
|
f"{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"
|
|
f'text = "START"\n'
|
|
f"button_color = Color(0.39, 1.0, 0.098, 1)\n"
|
|
f"enable_comms = true\n"
|
|
f'pushbutton_tag_name = "{name}_PB_OIP"\n'
|
|
f'lamp_tag_name = "{name}_LT_OIP"\n'
|
|
)
|
|
|
|
|
|
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}") |