Update workstation UI, ROS task control and device integration
This commit is contained in:
cheng
@@ -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)
|
||||
|
||||
+31
-42
@@ -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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user