Update workstation UI, ROS task control and device integration

2026-10-20 15:46:13 +08:00
parent 2b74c50ba8
commit e5b06df6da
141 changed files with 91520 additions and 748 deletions
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+"""Offline STEP -> triangle cache for Qt OpenGL. No robot/ROS/device operations.
+
+Requires gmsh (OpenCascade). Runtime GUI needs only numpy and the generated NPZ.
+"""
+import argparse
+import hashlib
+import json
+from pathlib import Path
+import gmsh
+import numpy as np
+
+
+def convert(source, output):
+    source=Path(source).resolve();output=Path(output).resolve()
+    gmsh.initialize(['step-converter','-nopopup'])
+    try:
+        gmsh.option.setNumber('General.Terminal',0)
+        gmsh.option.setString('Geometry.OCCTargetUnit','MM')
+        gmsh.model.add('gripper')
+        gmsh.model.occ.importShapes(str(source));gmsh.model.occ.synchronize()
+        gmsh.option.setNumber('Mesh.MeshSizeMin',.8)
+        gmsh.option.setNumber('Mesh.MeshSizeMax',2.)
+        gmsh.option.setNumber('Mesh.MeshSizeFromCurvature',18)
+        gmsh.option.setNumber('Mesh.ElementOrder',1)
+        gmsh.model.mesh.generate(2)
+        tags,coords,_=gmsh.model.mesh.getNodes()
+        order=np.argsort(tags);sorted_tags=tags[order]
+        nodes=np.asarray(coords).reshape(-1,3)[order]
+        parts=[];colors=[];labels=[]
+        surfaces=gmsh.model.getEntities(2)
+        for part,(_,tag) in enumerate(surfaces):
+            kinds,_,element_nodes=gmsh.model.mesh.getElements(2,tag)
+            r,g,b,a=gmsh.model.getColor(2,tag)
+            base=np.array([r,g,b],float)/255.
+            if np.max(base)-np.min(base)<.03: base=np.array([.76,.79,.83])
+            for kind,indices in zip(kinds,element_nodes):
+                if kind!=2: raise ValueError('Only first order triangles are supported')
+                indices=np.asarray(indices).reshape(-1,3)
+                triangles=nodes[np.searchsorted(sorted_tags,indices)]
+                parts.append(triangles/1000.)
+                colors.append(np.tile(base,(len(triangles),1)))
+                labels.append(np.full(len(triangles),part,np.int32))
+        triangles=np.concatenate(parts).astype(np.float32)
+        rgb=np.concatenate(colors).astype(np.float32)
+        if not np.isfinite(triangles).all() or not len(triangles):raise ValueError('Invalid mesh')
+        lo=triangles.min(axis=(0,1));hi=triangles.max(axis=(0,1))
+        output.parent.mkdir(parents=True,exist_ok=True)
+        np.savez_compressed(output,triangles=triangles,colors=rgb,surface=np.concatenate(labels))
+        info=dict(source=source.name,source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(),
+                  units='m',source_units='mm',triangles=len(triangles),surfaces=len(surfaces),
+                  solids=len(gmsh.model.getEntities(3)),bounds_m=[lo.tolist(),hi.tolist()],
+                  dimensions_mm=((hi-lo)*1000).tolist(),articulated=False,
+                  note='Original assembled STEP pose; no inferred joints or live finger animation.')
+        output.with_suffix('.json').write_text(json.dumps(info,ensure_ascii=False,indent=2)+'\n')
+        print(json.dumps(info,ensure_ascii=False))
+    finally:gmsh.finalize()
+
+if __name__=='__main__':
+    parser=argparse.ArgumentParser();parser.add_argument('source');parser.add_argument('output')
+    args=parser.parse_args();convert(args.source,args.output)
@@ -18,15 +18,34 @@ ACK = Path("/tmp/flexiv_movel_ack")
 DONE = Path("/tmp/flexiv_movel_done")


-def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
-    return (
+def format_request(
+    seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
+    zone: str = "", toler: int = -1,
+) -> str:
+    """一行请求。不带 ``zone`` 时仍是 6 个数，插孔和粗定位保持厂家默认到位。
+
+    微升降要带过渡区和到位等级。厂家默认 ``Z50``（50 mm）加等级 3，
+    几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``，
+    最严且仍会检查到位的等级是 1（0 表示不检查）。
+    """
+    if bool(zone) != (int(toler) >= 0):
+        raise ValueError("MoveL 过渡区和到位等级必须一起给")
+    if zone and (zone != "ZFine" or int(toler) != 1):
+        raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1，收到 {zone} {toler}")
+    line = (
        f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
-        f"{1 if keep_z else 0} {float(vel):.4f}\n"
+        f"{1 if keep_z else 0} {float(vel):.4f}"
    )
+    if zone:
+        line += f" {zone} {int(toler)}"
+    return line + "\n"


-def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
-    text = format_request(seq, x, y, z, keep_z, vel)
+def write_request(
+    seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
+    zone: str = "", toler: int = -1,
+) -> None:
+    text = format_request(seq, x, y, z, keep_z, vel, zone, toler)
    tmp = REQ.with_name(REQ.name + ".tmp")
    tmp.write_text(text, encoding="utf-8")
    tmp.replace(REQ)
@@ -1,48 +1,37 @@
 #!/usr/bin/env bash
-# =============================================================================
-# scripts/lib/ros_domain.sh —— 真机与仿真的 DDS 域隔离
-#
-# 为什么要单独抽一层
-# ----------------
-# 仿真侧（scripts/sim_env.sh）**强制** ROS_DOMAIN_ID=77，并且注释里写明了
-# "隔离不能靠『环境里恰好没设』"。真机侧当时没有对应保护，于是同一个坑换个
-# 方向又踩了一次（2026-09-28 实测）：
-#
-#   先 source 过 sim_env.sh 的 shell（ROS_DOMAIN_ID=77 还 export 着）
-#   再起真机栈 → 真机栈落到**仿真的域**上 → 两边节点名撞车
-#   （/controller_manager、/robot_state_publisher、/arm_status_node…）、
-#   /joint_states 与 /tf 各有两个来源互相盖。
-#
-#   现象极具误导性：控制器 feedback 与 reference 完全一致（控制器没坏），
-#   但 TF 显示末端**一动不动** —— 顺着现象查会一路查到 IK / TF / 时钟上，
-#   全是无辜的。
-#
-# 真机侧有两个入口（scripts/env.sh 给交互 shell 用；scripts/run_real.sh 自己
-# 起 conda + install），两处都得有这个纠正，所以放在这里共用一份。
-#
-# 用法（在 `set -uo pipefail` 或 `set -eo pipefail` 之后 source）：
-#     source ".../lib/ros_domain.sh"
-#     ros_domain_force_real
-# =============================================================================
-
-# 仿真的域。改这里要同步 scripts/sim_env.sh（那边是硬编码的 77）。
+# One explicit discovery policy for the station and every child launch.
+# The inherited generic ROS_DOMAIN_ID is never silently adopted.
 ROS_SIM_DOMAIN=77

-# 把 ROS_DOMAIN_ID 纠正到"真机该用的域"。
-#   · REAL_ROS_DOMAIN_ID 显式设了 → 用它（要换域就走这个，名字里带 REAL，
-#     不会被仿真环境的 export 撞上）；
-#   · 环境里是 77（仿真的域）→ **喊出来**再改回 0；
-#   · 环境里没设 → 显式写 0，让人一眼能从 `env` 里看出真机跑在哪个域。
 ros_domain_force_real() {
-    if [ -n "${REAL_ROS_DOMAIN_ID:-}" ]; then
-        export ROS_DOMAIN_ID="${REAL_ROS_DOMAIN_ID}"
-    elif [ "${ROS_DOMAIN_ID:-0}" = "${ROS_SIM_DOMAIN}" ]; then
-        echo "[domain] ⚠ 环境里的 ROS_DOMAIN_ID=${ROS_SIM_DOMAIN} 是**仿真**用的域" >&2
-        echo "[domain]   （scripts/sim_env.sh 强制那个值）。已改回 0 起真机栈 ——" >&2
-        echo "[domain]   真机与仿真同域会让节点名/话题互相盖，现象是'臂没动'" >&2
-        echo "[domain]   而控制器一切正常。要另指定请设 REAL_ROS_DOMAIN_ID。" >&2
-        export ROS_DOMAIN_ID=0
-    elif [ -z "${ROS_DOMAIN_ID:-}" ]; then
-        export ROS_DOMAIN_ID=0
+    local runtime_file requested_domain requested_localhost previous_domain
+    runtime_file="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)/config/ros_runtime.env"
+    requested_domain="${REAL_ROS_DOMAIN_ID:-}"
+    requested_localhost="${REAL_ROS_LOCALHOST_ONLY:-}"
+    if [[ -f "${runtime_file}" ]]; then
+        source "${runtime_file}"
+    fi
+    requested_domain="${requested_domain:-${ORISYS_ROS_DOMAIN_ID:-42}}"
+    requested_localhost="${requested_localhost:-${ORISYS_ROS_LOCALHOST_ONLY:-1}}"
+    if [[ ! "${requested_domain}" =~ ^[0-9]+$ ]] || (( 10#${requested_domain} > 232 )); then
+        echo "[domain] ERROR: ROS domain must be an integer in 0..232" >&2
+        return 2
+    fi
+    if [[ "${requested_localhost}" != 0 && "${requested_localhost}" != 1 ]]; then
+        echo "[domain] ERROR: REAL_ROS_LOCALHOST_ONLY must be 0 or 1" >&2
+        return 2
+    fi
+    if (( 10#${requested_domain} == ROS_SIM_DOMAIN )); then
+        echo "[domain] ERROR: domain 77 is reserved for simulation" >&2
+        return 2
+    fi
+    previous_domain="${ROS_DOMAIN_ID:-unset}"
+    export ROS_DOMAIN_ID="$((10#${requested_domain}))"
+    export ROS_LOCALHOST_ONLY="${requested_localhost}"
+    # All nested launch scripts inherit exactly the same selection.
+    export REAL_ROS_DOMAIN_ID="${ROS_DOMAIN_ID}"
+    export REAL_ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY}"
+    if [[ "${previous_domain}" != "${ROS_DOMAIN_ID}" ]]; then
+        echo "[domain] orisys_arm: ${previous_domain} -> ${ROS_DOMAIN_ID}; localhost_only=${ROS_LOCALHOST_ONLY}" >&2
    fi
 }