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

310 lines
8.6 KiB
Python
Raw Blame History

import csv
import math
from pathlib import Path
import re
# -----------------------
# CONFIG
# -----------------------
SCALE = 0.0254
FIXED_Y = 2.4
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"
# -----------------------
# LOAD CSV DEVICES
# -----------------------
devices = []
with open(CSV_PATH, newline="", encoding="utf-8") as f:
reader = csv.DictReader(f)
for row in reader:
if row.get("record_type") == "TPE":
devices.append(row)
if not devices:
raise RuntimeError("No TPE records found in CSV.")
devices_by_conveyor = {}
for d in devices:
key = (d.get("conveyor_key") or "").strip()
if key:
devices_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]
# -----------------------
# 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 SENSOR RESOURCE
# -----------------------
if 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}"]'
)
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:
"""
AutoCAD → Godot rotation conversion
AutoCAD:
0° = up
90° = right
180° = down
270° = left
clockwise
Godot:
0° = +X
90° = -Z
CCW
With Y→-Z flip already applied in coordinates.
"""
return math.radians((270 - cad_deg) % 360)
def compute_edge_offset(sensor_yaw, width):
"""
Move sensor to the edge in the direction perpendicular to where it's pointing.
The sensor beam points in the sensor_yaw direction, so we offset perpendicular to that.
"""
# Perpendicular to sensor's facing direction (90° to the right of where it points)
side = sensor_yaw - math.pi / 2
d = (width / 2) + EDGE_CLEARANCE
return math.cos(side) * d, math.sin(side) * d
def sensor_yaw_from_csv(info, conveyor_yaw, csv_x, csv_z):
cx, _, cz = info["position"]
# right perpendicular to conveyor forward
rx = math.cos(conveyor_yaw - math.pi / 2)
rz = math.sin(conveyor_yaw - math.pi / 2)
vx = csv_x - cx
vz = csv_z - cz
side = vx * rx + vz * rz
if side >= 0:
return conveyor_yaw + math.pi / 2
return conveyor_yaw - math.pi / 2
# -----------------------
# APPEND DEVICES
# -----------------------
node_blocks = []
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
csv_x = float(d["tpe_x"]) * SCALE
csv_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 we have ANY rotation data (block or TPE), use it
if tpe_rot != 0.0 or blk_rot != 0.0:
# Combine both rotations in CAD space
cad_world_rot = (blk_rot + tpe_rot) % 360.0
sensor_yaw = godot_yaw_from_cad(cad_world_rot)
else:
# fallback ONLY if both rotations are missing/zero
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'
)
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)} sensors added → {out_path}")
Reference in New Issue View Git Blame Copy Permalink