481 lines
16 KiB
Python
Executable File
481 lines
16 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import argparse
|
|
import json
|
|
import math
|
|
import pathlib
|
|
import subprocess
|
|
import sys
|
|
import time
|
|
|
|
|
|
AXES = ["X", "Y", "Z", "A", "B", "C", "U", "V", "W"]
|
|
SAMPLE_PERIOD_MS = 20
|
|
DEFAULT_SOURCE_ROOT = "/home/mes123456/cnc_wams/linuxcnc"
|
|
DEFAULT_INI = f"{DEFAULT_SOURCE_ROOT}/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini"
|
|
DEFAULT_PROGRAM = f"{DEFAULT_SOURCE_ROOT}/configs/sim/axis/vismach/5axis/table-rotary-tilting/demos/xyzbc_switchkins.ngc"
|
|
DEFAULT_OUTPUT = "/home/mes123456/cnc_wams/web-rtcp-5axis-xyzbc-trt-sim-plan/working/evidence/native-xyzbc-trt-evidence.json"
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description="Collect native LinuxCNC xyzbc-trt evidence as JSON.")
|
|
parser.add_argument("--ini", default=DEFAULT_INI)
|
|
parser.add_argument("--program", default=DEFAULT_PROGRAM)
|
|
parser.add_argument("--output", default=DEFAULT_OUTPUT)
|
|
parser.add_argument("--run", action="store_true", help="Optionally execute the program through linuxcnc.command().")
|
|
parser.add_argument("--timeout", type=float, default=60.0)
|
|
args = parser.parse_args()
|
|
|
|
try:
|
|
import linuxcnc
|
|
except Exception as exc:
|
|
write_json(args.output, {
|
|
"apiName": "xyzbc-trt-native-linuxcnc-evidence",
|
|
"status": "blocked",
|
|
"blocker": "python_linuxcnc_import_failed",
|
|
"error": f"{type(exc).__name__}: {exc}",
|
|
"hint": f"Run through {DEFAULT_SOURCE_ROOT}/scripts/rip-environment python3",
|
|
"sourceRoot": DEFAULT_SOURCE_ROOT,
|
|
"pathSampling": create_path_sampling(),
|
|
"previewPath": empty_path("linuxcnc-preview", "python linuxcnc import failed"),
|
|
"executionPath": empty_path("linuxcnc-stat", "python linuxcnc import failed"),
|
|
})
|
|
return 2
|
|
|
|
stat = linuxcnc.stat()
|
|
command = linuxcnc.command()
|
|
error_channel = linuxcnc.error_channel()
|
|
|
|
before = poll_stat(stat)
|
|
command_result = None
|
|
if args.run:
|
|
command_result = run_program(linuxcnc, stat, command, pathlib.Path(args.program), args.timeout)
|
|
|
|
after = poll_stat(stat)
|
|
hal = collect_hal_snapshot()
|
|
errors = drain_errors(error_channel)
|
|
|
|
evidence = {
|
|
"apiName": "xyzbc-trt-native-linuxcnc-evidence",
|
|
"status": "ok",
|
|
"collectedAt": iso_now(),
|
|
"executionMode": "auto-run" if args.run else "snapshot-only",
|
|
"sourceRoot": source_root_for_ini(args.ini),
|
|
"iniPath": args.ini,
|
|
"programPath": args.program,
|
|
"programExists": pathlib.Path(args.program).exists(),
|
|
"programLineCount": count_program_lines(args.program),
|
|
"linuxcncRuntime": {
|
|
"pythonApi": True,
|
|
"processes": list_processes(),
|
|
},
|
|
"before": before,
|
|
"after": after,
|
|
"commandResult": command_result,
|
|
"hal": hal,
|
|
"errors": errors,
|
|
"pathSampling": create_path_sampling(),
|
|
"previewPath": empty_path("linuxcnc-preview", "native AXIS preview/canon path capture is not implemented in this collector"),
|
|
"executionPath": execution_path_from_command(command_result),
|
|
"startupSequence": [
|
|
".desktop",
|
|
"rip-environment",
|
|
"linuxcncsvr",
|
|
"rtapi_app",
|
|
"milltask",
|
|
"halui",
|
|
"LIB:basic_sim.tcl",
|
|
"xyzbc-trt-kins",
|
|
"xyzbc-trt-gui",
|
|
"axis.py",
|
|
"xyzbc-trt.xml",
|
|
"switchkins_postgui.hal",
|
|
"OPEN_FILE ./demos/xyzbc_switchkins.ngc",
|
|
],
|
|
"halNets": native_hal_nets(hal),
|
|
"kinematicsPins": native_kinematics_pins(hal),
|
|
"coverage": {
|
|
"axisProfile": before.get("axisMask") == 55 or after.get("axisMask") == 55,
|
|
"xyzbcProgramOpen": str(after.get("file") or before.get("file") or "").endswith("xyzbc_switchkins.ngc"),
|
|
"switchkinsPinReadable": "motion.switchkins-type" in hal.get("pins", {}),
|
|
"jointFeedbackReadable": all(f"joint.{i}.pos-fb" in hal.get("pins", {}) for i in range(5)),
|
|
"taskStateReadable": after.get("taskState") is not None,
|
|
"positionReadable": bool(after.get("position")),
|
|
"previewPathAvailable": False,
|
|
"executionPathAvailable": command_result is not None and len(command_result.get("events", [])) > 0,
|
|
},
|
|
"semanticBoundary": "native_linuxcnc_axis_vismach_xyzbc_trt_runtime",
|
|
}
|
|
write_json(args.output, evidence)
|
|
print(f"native_xyzbc_trt_evidence={args.output}")
|
|
return 0
|
|
|
|
|
|
def run_program(linuxcnc, stat, command, program_path, timeout):
|
|
result = {
|
|
"requested": True,
|
|
"program": str(program_path),
|
|
"events": [],
|
|
"status": "unknown",
|
|
}
|
|
try:
|
|
command.state(linuxcnc.STATE_ESTOP_RESET)
|
|
command.wait_complete()
|
|
command.state(linuxcnc.STATE_ON)
|
|
command.wait_complete()
|
|
command.mode(linuxcnc.MODE_AUTO)
|
|
command.wait_complete()
|
|
command.program_open(str(program_path))
|
|
command.wait_complete()
|
|
command.auto(linuxcnc.AUTO_RUN, 0)
|
|
start = time.time()
|
|
while time.time() - start < timeout:
|
|
snapshot = poll_stat(stat)
|
|
result["events"].append({
|
|
"elapsedSeconds": round(time.time() - start, 3),
|
|
"interpState": snapshot.get("interpState"),
|
|
"execState": snapshot.get("execState"),
|
|
"currentLine": snapshot.get("currentLine"),
|
|
"readLine": snapshot.get("readLine"),
|
|
"position": snapshot.get("position"),
|
|
"jointActualPosition": snapshot.get("jointActualPosition"),
|
|
"velocity": snapshot.get("velocity"),
|
|
"feedrate": snapshot.get("feedrate"),
|
|
"spindle": snapshot.get("spindle"),
|
|
})
|
|
if snapshot.get("interpState") == linuxcnc.INTERP_IDLE and len(result["events"]) > 2:
|
|
result["status"] = "completed"
|
|
break
|
|
time.sleep(0.05)
|
|
else:
|
|
result["status"] = "timeout"
|
|
except Exception as exc:
|
|
result["status"] = "error"
|
|
result["error"] = f"{type(exc).__name__}: {exc}"
|
|
return result
|
|
|
|
|
|
def poll_stat(stat):
|
|
stat.poll()
|
|
return {
|
|
"taskState": value(stat, "task_state"),
|
|
"taskMode": value(stat, "task_mode"),
|
|
"interpState": value(stat, "interp_state"),
|
|
"execState": value(stat, "exec_state"),
|
|
"file": value(stat, "file"),
|
|
"currentLine": value(stat, "current_line"),
|
|
"readLine": value(stat, "read_line"),
|
|
"axisMask": value(stat, "axis_mask"),
|
|
"homed": tuple_to_list(value(stat, "homed")),
|
|
"position": axes_tuple(value(stat, "position")),
|
|
"actualPosition": axes_tuple(value(stat, "actual_position")),
|
|
"jointActualPosition": joint_tuple(value(stat, "joint_actual_position")),
|
|
"jointPosition": joint_tuple(value(stat, "joint_position")),
|
|
"dtg": axes_tuple(value(stat, "dtg")),
|
|
"velocity": value(stat, "current_vel"),
|
|
"feedrate": value(stat, "feedrate"),
|
|
"rapidrate": value(stat, "rapidrate"),
|
|
"spindle": normalize_json(value(stat, "spindle")),
|
|
}
|
|
|
|
|
|
def collect_hal_snapshot():
|
|
pins = {}
|
|
commands = [
|
|
["halcmd", "show", "pin", "motion.switchkins-type"],
|
|
["halcmd", "show", "pin", "motion.analog-out-03"],
|
|
["halcmd", "show", "pin", "motion.tooloffset.z"],
|
|
["halcmd", "show", "pin", "joint.0.pos-fb"],
|
|
["halcmd", "show", "pin", "joint.1.pos-fb"],
|
|
["halcmd", "show", "pin", "joint.2.pos-fb"],
|
|
["halcmd", "show", "pin", "joint.3.pos-fb"],
|
|
["halcmd", "show", "pin", "joint.4.pos-fb"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.tool-offset"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.x-offset"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.z-offset"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.x-rot-point"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.y-rot-point"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.z-rot-point"],
|
|
["halcmd", "show", "pin", "xyzbc-trt-kins.conventional-directions"],
|
|
]
|
|
raw = []
|
|
for command in commands:
|
|
completed = subprocess.run(command, text=True, capture_output=True)
|
|
raw.append({
|
|
"command": command,
|
|
"returncode": completed.returncode,
|
|
"stdout": completed.stdout,
|
|
"stderr": completed.stderr,
|
|
})
|
|
parse_halcmd_pins(completed.stdout, pins)
|
|
return {
|
|
"pins": pins,
|
|
"raw": raw,
|
|
}
|
|
|
|
|
|
def parse_halcmd_pins(text, pins):
|
|
for line in text.splitlines():
|
|
parts = line.split()
|
|
if len(parts) < 5:
|
|
continue
|
|
name = parts[4]
|
|
if "." not in name:
|
|
continue
|
|
pins[name] = {
|
|
"owner": parts[0],
|
|
"type": parts[1],
|
|
"direction": parts[2],
|
|
"value": parse_number(parts[3]),
|
|
"linked": "==>" in line or "<==" in line,
|
|
"raw": line,
|
|
}
|
|
|
|
|
|
def drain_errors(error_channel):
|
|
errors = []
|
|
for _ in range(20):
|
|
error = error_channel.poll()
|
|
if not error:
|
|
break
|
|
errors.append(normalize_json(error))
|
|
return errors
|
|
|
|
|
|
def list_processes():
|
|
completed = subprocess.run(
|
|
["ps", "-ef"],
|
|
text=True,
|
|
capture_output=True,
|
|
)
|
|
processes = []
|
|
for line in completed.stdout.splitlines():
|
|
if "xyzbc-trt" in line or "linuxcncsvr" in line or "milltask" in line or "halui -ini" in line:
|
|
processes.append(line)
|
|
return processes
|
|
|
|
|
|
def count_program_lines(path):
|
|
try:
|
|
return len(pathlib.Path(path).read_text(encoding="utf-8", errors="replace").splitlines())
|
|
except OSError:
|
|
return 0
|
|
|
|
|
|
def create_path_sampling():
|
|
return {
|
|
"samplePeriodMs": SAMPLE_PERIOD_MS,
|
|
"timeBase": "program-relative-ms",
|
|
"resampling": "linear-position-slerp-or-axis-linear",
|
|
"coordinateSystem": "machine-xyzbc-and-tcp",
|
|
}
|
|
|
|
|
|
def empty_path(source, reason):
|
|
return {
|
|
"source": source,
|
|
"samplePeriodMs": SAMPLE_PERIOD_MS,
|
|
"status": "blocked",
|
|
"unavailableReason": reason,
|
|
"sampleCount": 0,
|
|
"samples": [],
|
|
}
|
|
|
|
|
|
def execution_path_from_command(command_result):
|
|
events = (command_result or {}).get("events", [])
|
|
if not events:
|
|
return empty_path("linuxcnc-stat", "collector was run without --run or no execution events were captured")
|
|
samples = []
|
|
event_index = 0
|
|
max_ms = int(round(float(events[-1].get("elapsedSeconds") or 0) * 1000))
|
|
for sample_index, time_ms in enumerate(range(0, max_ms + SAMPLE_PERIOD_MS, SAMPLE_PERIOD_MS)):
|
|
while (
|
|
event_index + 1 < len(events)
|
|
and float(events[event_index + 1].get("elapsedSeconds") or 0) * 1000 <= time_ms
|
|
):
|
|
event_index += 1
|
|
event = events[event_index]
|
|
samples.append(path_sample_from_event(sample_index, time_ms, event))
|
|
return {
|
|
"source": "linuxcnc-stat",
|
|
"samplePeriodMs": SAMPLE_PERIOD_MS,
|
|
"status": "ok" if samples else "blocked",
|
|
"unavailableReason": None if samples else "no execution samples after resampling",
|
|
"sampleCount": len(samples),
|
|
"samples": samples,
|
|
}
|
|
|
|
|
|
def path_sample_from_event(sample_index, time_ms, event):
|
|
position = event.get("position") or {}
|
|
joints = event.get("jointActualPosition") or {}
|
|
b = number_or_zero(joints.get("3", position.get("b")))
|
|
c = number_or_zero(joints.get("4", position.get("c")))
|
|
joint = {
|
|
"x": number_or_zero(joints.get("0", position.get("x"))),
|
|
"y": number_or_zero(joints.get("1", position.get("y"))),
|
|
"z": number_or_zero(joints.get("2", position.get("z"))),
|
|
"b": b,
|
|
"c": c,
|
|
}
|
|
return {
|
|
"sampleIndex": sample_index,
|
|
"timeMs": time_ms,
|
|
"line": int(number_or_zero(event.get("currentLine"))),
|
|
"motionType": "unknown",
|
|
"activeKinematics": "unknown",
|
|
"tool": {
|
|
"id": 0,
|
|
"length": 0,
|
|
"diameter": 0,
|
|
},
|
|
"joint": joint,
|
|
"tcp": {
|
|
"x": joint["x"],
|
|
"y": joint["y"],
|
|
"z": joint["z"],
|
|
},
|
|
"toolAxis": tool_axis_from_bc(b, c),
|
|
"feed": number_or_zero(event.get("feedrate")),
|
|
"spindle": spindle_speed(event.get("spindle")),
|
|
}
|
|
|
|
|
|
def tool_axis_from_bc(b_deg, c_deg):
|
|
b = math.radians(number_or_zero(b_deg))
|
|
c = math.radians(number_or_zero(c_deg))
|
|
return {
|
|
"i": math.sin(b) * math.cos(c),
|
|
"j": math.sin(b) * math.sin(c),
|
|
"k": math.cos(b),
|
|
}
|
|
|
|
|
|
def spindle_speed(spindle):
|
|
if isinstance(spindle, list) and spindle:
|
|
first = spindle[0]
|
|
if isinstance(first, dict):
|
|
return number_or_zero(first.get("speed"))
|
|
if isinstance(spindle, dict):
|
|
return number_or_zero(spindle.get("speed"))
|
|
return 0
|
|
|
|
|
|
def native_hal_nets(hal):
|
|
pins = hal.get("pins", {})
|
|
return [
|
|
{
|
|
"signal": "kinstype-select",
|
|
"source": "motion.analog-out-03",
|
|
"target": "motion.switchkins-type",
|
|
"present": "motion.switchkins-type" in pins,
|
|
},
|
|
*[
|
|
{
|
|
"signal": f"joint-{index}-feedback",
|
|
"source": f"joint.{index}.pos-fb",
|
|
"target": ["table-x", "saddle-y", "spindle-z", "tilt-b", "rotate-c"][index],
|
|
"present": f"joint.{index}.pos-fb" in pins,
|
|
}
|
|
for index in range(5)
|
|
],
|
|
{
|
|
"signal": "tool-offset",
|
|
"source": "motion.tooloffset.z",
|
|
"target": "xyzbc-trt-kins.tool-offset",
|
|
"present": "motion.tooloffset.z" in pins or "xyzbc-trt-kins.tool-offset" in pins,
|
|
},
|
|
]
|
|
|
|
|
|
def native_kinematics_pins(hal):
|
|
pins = hal.get("pins", {})
|
|
return {
|
|
"xOffset": hal_pin_value(pins, "xyzbc-trt-kins.x-offset"),
|
|
"zOffset": hal_pin_value(pins, "xyzbc-trt-kins.z-offset"),
|
|
"xRotPoint": hal_pin_value(pins, "xyzbc-trt-kins.x-rot-point"),
|
|
"yRotPoint": hal_pin_value(pins, "xyzbc-trt-kins.y-rot-point"),
|
|
"zRotPoint": hal_pin_value(pins, "xyzbc-trt-kins.z-rot-point"),
|
|
"conventionalDirections": hal_pin_value(pins, "xyzbc-trt-kins.conventional-directions"),
|
|
"toolOffset": hal_pin_value(pins, "xyzbc-trt-kins.tool-offset"),
|
|
}
|
|
|
|
|
|
def hal_pin_value(pins, name):
|
|
return (pins.get(name) or {}).get("value")
|
|
|
|
|
|
def source_root_for_ini(ini_path):
|
|
marker = "/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini"
|
|
value = str(ini_path)
|
|
if value.endswith(marker):
|
|
return value[: -len(marker)]
|
|
return DEFAULT_SOURCE_ROOT
|
|
|
|
|
|
def value(obj, attr):
|
|
return normalize_json(getattr(obj, attr, None))
|
|
|
|
|
|
def axes_tuple(values):
|
|
values = tuple_to_list(values)
|
|
return {axis.lower(): values[index] for index, axis in enumerate(AXES) if index < len(values)}
|
|
|
|
|
|
def joint_tuple(values):
|
|
values = tuple_to_list(values)
|
|
return {str(index): values[index] for index in range(min(5, len(values)))}
|
|
|
|
|
|
def tuple_to_list(values):
|
|
if values is None:
|
|
return []
|
|
return [normalize_json(value) for value in values]
|
|
|
|
|
|
def normalize_json(value):
|
|
if isinstance(value, float):
|
|
if math.isnan(value) or math.isinf(value):
|
|
return None
|
|
return value
|
|
if isinstance(value, (str, int, bool)) or value is None:
|
|
return value
|
|
if isinstance(value, tuple):
|
|
return [normalize_json(item) for item in value]
|
|
if isinstance(value, list):
|
|
return [normalize_json(item) for item in value]
|
|
if isinstance(value, dict):
|
|
return {str(key): normalize_json(item) for key, item in value.items()}
|
|
return str(value)
|
|
|
|
|
|
def parse_number(value):
|
|
try:
|
|
return float(value)
|
|
except ValueError:
|
|
return value
|
|
|
|
|
|
def number_or_zero(value):
|
|
try:
|
|
number = float(value)
|
|
except (TypeError, ValueError):
|
|
return 0
|
|
if math.isnan(number) or math.isinf(number):
|
|
return 0
|
|
return number
|
|
|
|
|
|
def write_json(path, payload):
|
|
output = pathlib.Path(path)
|
|
output.parent.mkdir(parents=True, exist_ok=True)
|
|
output.write_text(json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
|
|
|
|
|
def iso_now():
|
|
return time.strftime("%Y-%m-%dT%H:%M:%S%z")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|