Initial import of ORISYS arm workstation
This commit is contained in:
cheng
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# conda 环境切换的公共逻辑。被 scripts/env.sh 与 scripts/sim_env.sh 共用。
|
||||
#
|
||||
# 为什么需要这个：conda 的 activate 是**堆叠**的。在同一个 shell 里反复
|
||||
# activate / 切换不同环境，会不断往 PATH、PYTHONPATH 里追加条目，而不会
|
||||
# 清掉前一个环境留下的。实测的后果非常隐蔽：
|
||||
#
|
||||
# 在同一个 shell 里先 activate ros_gz_sim 再切 ros_flexiv 之后，
|
||||
# `which python` 指向 ros_flexiv/bin/python，
|
||||
# 但 `python -m pip` 却解析到 ros_gz_sim 的 pip！
|
||||
# 于是 colcon --symlink-install 报：
|
||||
# error: option --editable not recognized
|
||||
# 这个报错和真实原因（shell 脏了）毫无关系，极易被误判成
|
||||
# "环境被搞坏了 / setuptools 版本不对"，进而去做一堆无用的修复。
|
||||
#
|
||||
# 对策：切换前先把堆叠的激活全部退掉（退到 base），再激活目标环境。
|
||||
# =============================================================================
|
||||
|
||||
# conda_deactivate_all: 退到 base（CONDA_SHLVL=1）。任何环境下调用都安全。
|
||||
conda_deactivate_all() {
|
||||
local guard=0
|
||||
# CONDA_SHLVL 是 conda 自己维护的激活层数；base 为 1。
|
||||
# 加 guard 是为了防止某些畸形状态下无限循环。
|
||||
while [[ "${CONDA_SHLVL:-1}" -gt 1 && "${guard}" -lt 20 ]]; do
|
||||
conda deactivate 2>/dev/null || break
|
||||
guard=$((guard + 1))
|
||||
done
|
||||
return 0
|
||||
}
|
||||
|
||||
# conda_switch_clean <env_name> [<other_env_name>]
|
||||
# 干净地激活 <env_name>；若当前已在 <other_env_name> 里，先退到 base 再切，
|
||||
# 并提示用户最好开新 shell。
|
||||
# 返回 0 = 成功；非 0 = 目标环境不存在。
|
||||
conda_switch_clean() {
|
||||
local target="$1"
|
||||
local other="${2:-}"
|
||||
|
||||
if [[ -n "${other}" && "${CONDA_DEFAULT_ENV:-}" == "${other}" ]]; then
|
||||
echo "提示: 当前 shell 激活的是 ${other}，即将切到 ${target}。" >&2
|
||||
echo " conda 的激活是堆叠的，同一 shell 里来回切会残留 PATH/PYTHONPATH，" >&2
|
||||
echo " 典型症状是构建报 \"--editable not recognized\" 这类与真实原因无关的错。" >&2
|
||||
echo " 这里会先退到 base 再切以规避；但最稳妥仍是开一个新 shell。" >&2
|
||||
fi
|
||||
|
||||
conda_deactivate_all
|
||||
|
||||
if ! conda activate "${target}" 2>/dev/null; then
|
||||
return 1
|
||||
fi
|
||||
return 0
|
||||
}
|
||||
|
||||
# path_drop_patterns <VAR> <glob>...
|
||||
# 从环境变量 <VAR> 的冒号分隔列表里删掉所有匹配任一 glob 的条目（保持剩余顺序）。
|
||||
# 用于清理"同一个 shell 里先 source 过另一套环境"留下的路径残留。
|
||||
#
|
||||
# 为什么必须清：真机环境（scripts/env.sh）与仿真环境（scripts/sim_env.sh）
|
||||
# 共用同一个工作区，各自的 setup.bash 都会往 PATH / PYTHONPATH /
|
||||
# LD_LIBRARY_PATH / AMENT_PREFIX_PATH 里追加自己的产物目录。conda 的
|
||||
# deactivate 只会撤掉 conda 自己那部分，**不会**撤掉 setup.bash 追加的条目。
|
||||
# 于是"先真机后仿真"的 shell 里会长成这样：
|
||||
# PYTHONPATH = .../install/arm_msgs/... : .../install_sim/arm_msgs/...
|
||||
# 此时 `import arm_msgs` 会拿到**真机那份**，与 install_sim 下的运行库混用，
|
||||
# 症状是消息反序列化莫名失败、字段对不上、dlopen 找不到 typesupport ——
|
||||
# 报错信息完全不提"环境脏了"这个真实原因。
|
||||
#
|
||||
# 实测踩过：sim_tactile_adapter 订阅 ros_gz_interfaces/msg/Contacts 时崩在
|
||||
# rcutils_set_error_state: Type support not from this implementation ...
|
||||
# Could not load library libros_gz_interfaces__rosidl_typesupport_fastrtps_c.so
|
||||
# 库其实就在 $CONDA_PREFIX/lib 下（ldd 全齐），根因是 LD_LIBRARY_PATH 里
|
||||
# 既残留着 ros_flexiv 的 lib、又**没有** $CONDA_PREFIX/lib。
|
||||
path_drop_patterns() {
|
||||
local _var="$1"
|
||||
shift
|
||||
local -a _pats=("$@")
|
||||
local _cur="${!_var:-}"
|
||||
[[ -z "${_cur}" ]] && return 0
|
||||
|
||||
local _out="" _entry _pat _hit
|
||||
local IFS=':'
|
||||
for _entry in ${_cur}; do
|
||||
_hit=0
|
||||
for _pat in "${_pats[@]}"; do
|
||||
# 故意不加引号：让右侧当 glob 用（这里就是要通配匹配）
|
||||
# shellcheck disable=SC2053
|
||||
if [[ "${_entry}" == ${_pat} ]]; then
|
||||
_hit=1
|
||||
break
|
||||
fi
|
||||
done
|
||||
[[ "${_hit}" -eq 1 ]] && continue
|
||||
_out="${_out:+${_out}:}${_entry}"
|
||||
done
|
||||
|
||||
printf -v "${_var}" '%s' "${_out}"
|
||||
export "${_var?}"
|
||||
return 0
|
||||
}
|
||||
|
||||
# 供调用方按需取消设置，避免污染使用者的变量空间。
|
||||
# （bash 没有"私有函数"概念，命名上统一加 conda_ 前缀降低撞名概率。）
|
||||
@@ -0,0 +1,31 @@
|
||||
# 本机工具链。仓库位置用调用方已经算好的 ROOT，不写死某个用户的家目录。
|
||||
# 用法：source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
|
||||
dev_source_conda() {
|
||||
local cand
|
||||
for cand in \
|
||||
"${HOME}/anaconda3/etc/profile.d/conda.sh" \
|
||||
"${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
do
|
||||
if [[ -f "${cand}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${cand}"
|
||||
return 0
|
||||
fi
|
||||
done
|
||||
echo "ERROR: 找不到 conda.sh。试过 \$HOME/anaconda3 和 \$HOME/miniconda3。" >&2
|
||||
return 1
|
||||
}
|
||||
|
||||
dev_rdk_python() {
|
||||
if [[ -n "${RDK_PYTHON:-}" && -x "${RDK_PYTHON}" ]]; then
|
||||
printf '%s\n' "${RDK_PYTHON}"
|
||||
return 0
|
||||
fi
|
||||
if [[ -x "${HOME}/rdk_py310/bin/python" ]]; then
|
||||
printf '%s\n' "${HOME}/rdk_py310/bin/python"
|
||||
return 0
|
||||
fi
|
||||
echo "ERROR: 找不到 RDK 的 Python。设置 RDK_PYTHON，或装到 \$HOME/rdk_py310。" >&2
|
||||
return 1
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,82 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# 修 console_scripts 的 shebang（真机 / 仿真两套构建产物都会踩的坑）
|
||||
# =============================================================================
|
||||
#
|
||||
# 现象：
|
||||
# source scripts/sim_env.sh && ros2 run arm_sim_gazebo sim_camera_adapter
|
||||
# 进程列表里却是
|
||||
# /home/cc/anaconda3/envs/ros_flexiv/bin/python3.11 ... sim_camera_adapter
|
||||
# 即**仿真节点跑在真机的 conda 环境里**。
|
||||
#
|
||||
# 为什么：
|
||||
# setuptools 的 console_scripts 入口脚本，shebang 用的是**构建那一刻**的
|
||||
# sys.executable。本工作区只有一份源码，真机与仿真各有自己的 conda 环境
|
||||
# （ros_flexiv / ros_gz_sim）和各自的 install 目录。如果在真机环境里跑过
|
||||
# build_sim.sh（或反之），写进 install_sim/ 的脚本 shebang 就指向了另一个
|
||||
# 环境。`ros2 run` 是**照 shebang 执行**的，不看当前 CONDA_PREFIX。
|
||||
#
|
||||
# 为什么危险：
|
||||
# 两个环境都是 ROS 2 Humble + Python 3.11，所以多数时候"居然能跑"，
|
||||
# 只是 import 到的是另一个环境的包。表现为偶发的 ABI/`dlopen`/消息反序列化
|
||||
# 报错，报错信息完全不提"shebang 指错环境"。
|
||||
# 实测：LD_LIBRARY_PATH、PYTHONPATH 都清干净了，节点仍然跑在 ros_flexiv 里。
|
||||
#
|
||||
# 对策：
|
||||
# 构建收尾时扫一遍本次产物，把指向"别的 conda 环境"的 shebang 改回
|
||||
# **本环境**的 python。已指向本环境的文件不动（幂等）。
|
||||
#
|
||||
# 用法：
|
||||
# fix_shebangs <install_dir> <expected_python_path>
|
||||
# =============================================================================
|
||||
|
||||
# fix_shebangs <install_dir> <expected_python>
|
||||
# 把 <install_dir> 下所有 console_scripts 里"指向其它 conda 环境"的 shebang
|
||||
# 改写成 <expected_python>。返回 0；打印改了哪些文件（没改就不打印）。
|
||||
fix_shebangs() {
|
||||
local install_dir="$1"
|
||||
local expected="$2"
|
||||
|
||||
if [[ ! -d "${install_dir}" || ! -x "${expected}" ]]; then
|
||||
return 0
|
||||
fi
|
||||
|
||||
# 只扫这两类位置：包自带的 console_scripts 目录、以及 bin/。
|
||||
# 用 find 而不是递归 rg：要按"是不是可执行文件 + 首行是 shebang"判断。
|
||||
local -a files=()
|
||||
local f
|
||||
while IFS= read -r -d '' f; do
|
||||
files+=("${f}")
|
||||
done < <(find "${install_dir}" \
|
||||
\( -path "*/lib/*" -o -path "*/bin/*" \) \
|
||||
-type f -perm -u+x -print0 2>/dev/null)
|
||||
|
||||
local fixed=0 first
|
||||
for f in "${files[@]}"; do
|
||||
# 读首行（最多 200 字节）；不是 #! 开头就跳过（二进制、数据文件等）
|
||||
first="$(head -c 200 "${f}" | head -1)"
|
||||
[[ "${first:0:2}" == "#!" ]] || continue
|
||||
# 只动"指向某个 conda 环境"的 shebang；/usr/bin/python3 这类系统解释器不动
|
||||
[[ "${first}" == *"/envs/"*"python"* ]] || continue
|
||||
[[ "${first}" == *"${expected}"* ]] && continue
|
||||
|
||||
# 保留原 shebang 里的参数（例如 `python3 -u`）
|
||||
local interp="${first#\#!}" # 去掉 #!
|
||||
interp="${interp%% *}" # 第一个 token = 解释器路径
|
||||
local rest="${first#"#!${interp}"}" # 剩下的是参数（含前导空格）
|
||||
# 就地重写首行：用 printf + tail 组装，避免 sed -i 在符号链接上的坑
|
||||
local tmp="${f}.shebang.$$"
|
||||
{
|
||||
printf '#!%s%s\n' "${expected}" "${rest}"
|
||||
tail -n +2 "${f}"
|
||||
} > "${tmp}" && cat "${tmp}" > "${f}" && rm -f "${tmp}"
|
||||
echo " 修正 shebang: ${f#$PWD/} → ${expected}"
|
||||
fixed=$((fixed + 1))
|
||||
done
|
||||
|
||||
if [[ "${fixed}" -gt 0 ]]; then
|
||||
echo " （共修正 ${fixed} 个入口脚本：它们的 shebang 原本指向别的 conda 环境，"
|
||||
echo " 会导致 ros2 run 起来的节点跑在错误的环境里）"
|
||||
fi
|
||||
return 0
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
"""伺服和 flexiv_hardware 之间的 MoveL 交接。
|
||||
|
||||
两边不在一个进程里，硬件接口也没有自己的 ROS 节点。用三个文件：
|
||||
|
||||
* ``/tmp/flexiv_movel_req`` 一行 ``seq x y z keep_z vel``
|
||||
* ``/tmp/flexiv_movel_ack`` 硬件接住请求后立刻写序号
|
||||
* ``/tmp/flexiv_movel_done`` 一行 ``seq ok`` 或 ``seq fail 原因``
|
||||
|
||||
先写临时文件再改名，避免对面读到半行。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
REQ = Path("/tmp/flexiv_movel_req")
|
||||
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 (
|
||||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}\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)
|
||||
tmp = REQ.with_name(REQ.name + ".tmp")
|
||||
tmp.write_text(text, encoding="utf-8")
|
||||
tmp.replace(REQ)
|
||||
|
||||
|
||||
def parse_done(text: str):
|
||||
"""返回 ``(seq, ok, detail)``。行不完整时返回 None。"""
|
||||
parts = text.strip().split(maxsplit=2)
|
||||
if len(parts) < 2:
|
||||
return None
|
||||
try:
|
||||
seq = int(parts[0])
|
||||
except ValueError:
|
||||
return None
|
||||
if parts[1] not in ("ok", "fail"):
|
||||
return None
|
||||
detail = parts[2] if len(parts) > 2 else ""
|
||||
return seq, parts[1] == "ok", detail
|
||||
|
||||
|
||||
def read_done():
|
||||
try:
|
||||
text = DONE.read_text(encoding="utf-8")
|
||||
except OSError:
|
||||
return None
|
||||
return parse_done(text)
|
||||
|
||||
|
||||
def read_ack() -> int:
|
||||
try:
|
||||
text = ACK.read_text(encoding="utf-8").strip()
|
||||
except OSError:
|
||||
return -1
|
||||
try:
|
||||
return int(text.split()[0])
|
||||
except (ValueError, IndexError):
|
||||
return -1
|
||||
Executable
+50
@@ -0,0 +1,50 @@
|
||||
#!/usr/bin/env bash
|
||||
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
|
||||
# 用法（在其它脚本里）：
|
||||
# source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
#
|
||||
# 本文件同时负责**在配置有问题时立刻拦下**，因为它的调用方几乎都是
|
||||
# `eval "$(...)"` 或直接引用 `SITE_*`：如果这里不拦，脚本会拿着一堆空变量
|
||||
# 继续往下跑，真正报错的地方会离原因很远（历史上就这么出过"参数静默失效"）。
|
||||
#
|
||||
# 两种"没配置好"要区别对待：
|
||||
# 1. site.yaml **不存在** —— 新克隆还没 `cp`。给 WARN 就放行，
|
||||
# 因为有些脚本（网络检查、RDK 自检）在配置前就该能用。
|
||||
# 2. site.yaml **存在但选不出机械臂** —— 配置写错了。直接拦下，
|
||||
# 并且必须给出"改哪个文件的哪个键"。
|
||||
|
||||
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
|
||||
_SITE_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
|
||||
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
|
||||
|
||||
# 供调用方判断"是没配置还是配错了"。0 = 正常加载；1 = 文件不存在；2 = 配置有误。
|
||||
SITE_LOAD_STATUS=0
|
||||
|
||||
if [[ ! -f "${_SITE_FILE}" ]]; then
|
||||
echo "[site] WARN: 没有 ${_SITE_FILE}（新克隆请先 cp config/site.example.yaml config/site.yaml）" >&2
|
||||
SITE_LOAD_STATUS=1
|
||||
else
|
||||
_site_env=""
|
||||
if ! _site_env="$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"; then
|
||||
echo "[site] ERROR: 解析 ${_SITE_FILE} 失败，见上面 export_site_env.py 的报错。" >&2
|
||||
echo "[site] 修好后重试；不要带着空的 SITE_* 继续跑。" >&2
|
||||
SITE_LOAD_STATUS=2
|
||||
else
|
||||
# shellcheck disable=SC1090
|
||||
eval "${_site_env}"
|
||||
export SITE_CONFIG="${_SITE_FILE}"
|
||||
|
||||
if [[ -z "${SITE_ROBOT_SN:-}" ]]; then
|
||||
# 绝不兜底成某个 SN：那等于"配置漏了照样启动，连的是哪台看运气"。
|
||||
echo "[site] ERROR: ${_SITE_FILE} 里没有选出机械臂（robot.sn 为空且没有 --arm）。" >&2
|
||||
echo "[site] 在 config/site.yaml 的 robot.sn 里填完整 SN，" >&2
|
||||
echo "[site] 或给命令加 --arm <臂号>（臂号 = 该臂 SN 的末两位）。" >&2
|
||||
SITE_LOAD_STATUS=2
|
||||
else
|
||||
# 把 89/91/left/right 别名解析成完整的 Flexiv SN，供 RDK 使用。
|
||||
export ROBOT_SN="${SITE_ROBOT_SN}"
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
unset _SITE_ROOT _SITE_EXPORTER _SITE_FILE _site_env
|
||||
@@ -0,0 +1,26 @@
|
||||
#!/usr/bin/env bash
|
||||
# 从 argv 里解析 --sn / --arm。必须在 `load_site_config.sh` 之前 source，
|
||||
# 这样 `ROBOT_SN` 才能被 export_site_env.py 看到。
|
||||
#
|
||||
# source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
# parse_arm_args "$@"
|
||||
# set -- "${PARSE_ARM_REST[@]}"
|
||||
#
|
||||
# 别名（89|91|left|right）在 export_site_env.py 里解析。
|
||||
|
||||
parse_arm_args() {
|
||||
PARSE_ARM_REST=()
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--sn|--arm)
|
||||
export ROBOT_SN="${2:?}"
|
||||
export ROBOT_ARM="${2}"
|
||||
shift 2
|
||||
;;
|
||||
*)
|
||||
PARSE_ARM_REST+=("$1")
|
||||
shift
|
||||
;;
|
||||
esac
|
||||
done
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#!/usr/bin/env bash
|
||||
# 由两个 Qt 启动器 source；独立目录固定 PySide6 和 shiboken，不替换 ROS 环境的包。
|
||||
orisys_qt_env() {
|
||||
local qt_version="6.8.3" qt_python qt_dir qt_temp
|
||||
qt_python="$(python3 -c 'import sys; print(f"{sys.version_info.major}{sys.version_info.minor}")')"
|
||||
qt_dir="${XDG_DATA_HOME:-${HOME}/.local/share}/orisys_arm/qt-${qt_version}-py${qt_python}"
|
||||
if ! PYTHONPATH="${qt_dir}" python3 -c 'import PySide6, shiboken6; assert PySide6.__version__ == shiboken6.__version__ == "6.8.3"' >/dev/null 2>&1; then
|
||||
echo "[Qt] 首次安装工位独立界面依赖 PySide6 ${qt_version}……" >&2
|
||||
mkdir -p "$(dirname "${qt_dir}")"
|
||||
qt_temp="$(mktemp -d "${qt_dir}.XXXXXX")"
|
||||
if ! python3 -m pip install --disable-pip-version-check --target "${qt_temp}" -r "${ROOT}/station/requirements-qt.txt"; then
|
||||
rm -rf "${qt_temp}"
|
||||
echo "[Qt] 界面依赖安装失败，请检查网络后重新启动。" >&2
|
||||
return 1
|
||||
fi
|
||||
rm -rf "${qt_dir}"
|
||||
mv "${qt_temp}" "${qt_dir}"
|
||||
fi
|
||||
export PYTHONPATH="${qt_dir}${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# 避免继承 OpenCV 等组件设置的其他 Qt 版本插件目录。
|
||||
unset QT_PLUGIN_PATH QT_QPA_PLATFORM_PLUGIN_PATH
|
||||
}
|
||||
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RDK 操作公共函数（必须在 rdk_py310 / Python 3.10 下运行）。
|
||||
|
||||
背景与约束
|
||||
----------
|
||||
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，所以所有
|
||||
直接调用 RDK 的代码必须以子进程方式跑在 `/home/cc/rdk_py310/bin/python` 下。
|
||||
因此本文件不能用任何 3.11+ 语法，也不 import ROS。
|
||||
|
||||
抽取原因
|
||||
--------
|
||||
`ensure_operational` / `ensure_idle` / tool 相关函数原先在
|
||||
`scripts/rdk_switch_tool.py` 与 `scripts/rdk_drag_bridge.py` 里各写了一份，
|
||||
行为已经出现细微差异（一个用 ServoOn 一个用 Enable，超时时间不同）。
|
||||
统一到本文件后，两处调用方行为一致。
|
||||
|
||||
重要限制
|
||||
--------
|
||||
RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一个 SN 会失败或行为异常。
|
||||
所以框架里所有 RDK 操作都应当经由 `rdk_drag_bridge.py` 这一个长驻进程串行化，
|
||||
不要让业务节点各自起 RDK 连接。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import time
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 使能与故障
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def ensure_operational(robot: Any, timeout_s: float = 10.0) -> Optional[str]:
|
||||
"""确保机器人处于 operational（已使能、无故障）。
|
||||
|
||||
返回 ``None`` 表示成功，否则返回给用户看的错误说明。
|
||||
|
||||
这是幂等的：已经 operational 时不做任何事。
|
||||
"""
|
||||
try:
|
||||
if robot.fault():
|
||||
robot.ClearFault()
|
||||
time.sleep(0.5)
|
||||
if not robot.operational():
|
||||
# 不同 RDK 版本一个叫 ServoOn 一个叫 Enable。
|
||||
for fn in ("ServoOn", "Enable"):
|
||||
if hasattr(robot, fn):
|
||||
try:
|
||||
getattr(robot, fn)()
|
||||
break
|
||||
except Exception:
|
||||
pass
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while not robot.operational() and time.monotonic() < deadline:
|
||||
time.sleep(0.2)
|
||||
if not robot.operational():
|
||||
return (
|
||||
"机器人未处于 operational 状态（故障 / 急停未复位？）。"
|
||||
"请在示教器上清故障、松开急停，并确认处于 AUTO(Remote)。"
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return str(exc)
|
||||
return None
|
||||
|
||||
|
||||
def clear_fault(robot: Any) -> Optional[str]:
|
||||
"""清除故障。返回 ``None`` 表示成功。"""
|
||||
try:
|
||||
robot.ClearFault()
|
||||
time.sleep(0.5)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return str(exc)
|
||||
return None
|
||||
|
||||
|
||||
def ensure_idle(robot: Any, mode: Any, timeout_s: float = 10.0) -> Optional[str]:
|
||||
"""确保机器人切到 IDLE 模式（工具切换等操作的前提）。
|
||||
|
||||
返回 ``None`` 表示成功，否则返回错误说明。
|
||||
"""
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
return None
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
err = ensure_operational(robot, timeout_s=timeout_s)
|
||||
if err:
|
||||
return err
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return f"无法切到 IDLE: {exc}"
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 工具坐标系
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def single_arm_groups(robot: Any) -> List[Any]:
|
||||
"""返回工具 API 的关节组列表。
|
||||
|
||||
RDK 1.9 的 Tool API 是单臂的（没有 JointGroup 概念），此时返回 ``[None]``
|
||||
作为哨兵，调用方据此走不带 group 参数的分支。老版本返回实际组名列表。
|
||||
"""
|
||||
try:
|
||||
info = robot.info()
|
||||
groups = getattr(info, "single_arm_groups", None) or {}
|
||||
keys = list(groups.keys()) if isinstance(groups, dict) else list(groups)
|
||||
if keys:
|
||||
return keys
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
if hasattr(flexivrdk, "JointGroup"):
|
||||
return [flexivrdk.JointGroup.ARM_1]
|
||||
except Exception:
|
||||
pass
|
||||
return [None]
|
||||
|
||||
|
||||
def tool_name(tool: Any, group: Any) -> str:
|
||||
"""读当前工具名。"""
|
||||
try:
|
||||
return str(tool.name() if group is None else tool.name(group))
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
|
||||
def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
|
||||
"""读当前工具参数，归一化成普通 dict。
|
||||
|
||||
返回 ``{"mass_kg": float, "com_m": [x,y,z], "tcp_m": [x,y,z]}``。
|
||||
读不到的字段用 0 / 空列表。
|
||||
"""
|
||||
try:
|
||||
params = tool.params() if group is None else tool.params(group)
|
||||
except Exception:
|
||||
return {"mass_kg": 0.0, "com_m": [], "tcp_m": []}
|
||||
|
||||
def _floats(value: Any) -> List[float]:
|
||||
try:
|
||||
return [float(v) for v in list(value)[:3]]
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
return {
|
||||
"mass_kg": float(getattr(params, "mass", 0.0) or 0.0),
|
||||
"com_m": _floats(getattr(params, "CoM", []) or []),
|
||||
"tcp_m": _floats(getattr(params, "tcp_location", []) or []),
|
||||
}
|
||||
|
||||
|
||||
def iface_ipv4s(name: str) -> List[str]:
|
||||
"""网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
|
||||
import fcntl
|
||||
import socket
|
||||
import struct
|
||||
import subprocess
|
||||
|
||||
name = (name or "").strip()
|
||||
if not name:
|
||||
return []
|
||||
if name.count(".") == 3:
|
||||
return [name]
|
||||
addrs: List[str] = []
|
||||
try:
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
||||
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
||||
s.close()
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
out = subprocess.check_output(
|
||||
["ip", "-4", "-o", "addr", "show", "dev", name],
|
||||
text=True,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
for line in out.splitlines():
|
||||
parts = line.split()
|
||||
if "inet" in parts:
|
||||
addrs.append(parts[parts.index("inet") + 1].split("/")[0])
|
||||
except Exception:
|
||||
pass
|
||||
seen = set()
|
||||
uniq: List[str] = []
|
||||
for a in addrs:
|
||||
if a and a not in seen:
|
||||
seen.add(a)
|
||||
uniq.append(a)
|
||||
return uniq
|
||||
|
||||
|
||||
def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
|
||||
"""给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
|
||||
|
||||
只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
|
||||
症状是 ping 通、RDK 报 Failed to connect。
|
||||
"""
|
||||
lists: List[List[str]] = []
|
||||
pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
|
||||
if pc.count(".") == 3:
|
||||
lists.append([pc])
|
||||
iface = (iface or "").strip()
|
||||
if iface.count(".") == 3:
|
||||
lists.append([iface])
|
||||
elif iface:
|
||||
lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
|
||||
if lan:
|
||||
lists.append(lan)
|
||||
all_ips = iface_ipv4s(iface)
|
||||
if all_ips and all_ips != lan:
|
||||
lists.append(all_ips)
|
||||
seen = set()
|
||||
out: List[List[str]] = []
|
||||
for item in lists:
|
||||
key = tuple(item)
|
||||
if key not in seen:
|
||||
seen.add(key)
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
def switch_tool(tool: Any, group: Any, target: str) -> None:
|
||||
"""切换工具。失败抛异常。"""
|
||||
if group is None:
|
||||
tool.Switch(target)
|
||||
else:
|
||||
tool.Switch(group, target)
|
||||
|
||||
|
||||
def resolve_tool_name(tool: Any, want: str) -> str:
|
||||
"""把用户给的模糊名字解析成 Studio 工具池里的准确名字。
|
||||
|
||||
大小写不敏感。找不到时抛 ``ValueError``，消息里列出全部可选项 ——
|
||||
这样操作员能立刻看到拼错在哪，而不是面对一个"切换失败"。
|
||||
"""
|
||||
want = (want or "").strip()
|
||||
if not want:
|
||||
return ""
|
||||
try:
|
||||
names = list(tool.list() or [])
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise ValueError(f"无法读取 Studio 工具列表: {exc}") from exc
|
||||
|
||||
if want in names:
|
||||
return want
|
||||
lower = {str(n).lower(): str(n) for n in names}
|
||||
hit = lower.get(want.lower())
|
||||
if hit:
|
||||
return hit
|
||||
raise ValueError(
|
||||
f"工具 '{want}' 不在 Studio 工具池里。可选: "
|
||||
+ (", ".join(str(n) for n in names) if names else "（空）")
|
||||
)
|
||||
Executable
+139
@@ -0,0 +1,139 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/real_procs.sh —— 真机进程扫描（被 stop_real.sh / run_real.sh 共用）
|
||||
#
|
||||
# 为什么要有这个文件
|
||||
# ----------------
|
||||
# 仿真侧早就有 lib/sim_procs.sh（点名表 + install_sim/ 兜底 + 杀完复查），
|
||||
# 真机侧**一直没有**对应物：只有 run_real.sh 里两行 pkill，
|
||||
# pkill -f '/controller_manager/ros2_control_node'
|
||||
# pkill -f 'arm_bringup.*stack.launch'
|
||||
# 而真机栈里还有一族**独立于这两条存活**的进程。2026-09-28 实测抓到的一次：
|
||||
# 上一轮栈的主体早就没了，只剩
|
||||
# real_trajectory_adapter / named_pose_server / arm_status_node /
|
||||
# joint_state_publisher
|
||||
# 四条（全在 install/arm_control/lib/arm_control/ 下），两条 pkill 一条都匹配不到。
|
||||
#
|
||||
# 危害不是"占内存"，而是**冒名顶替**：
|
||||
# · 残留的 adapter 照样订阅 /trajectory、
|
||||
# · 残留的 named_pose_server 照样提供 /set_named_pose、
|
||||
# · 残留的 joint_state_publisher 照样发 /joint_states（mock 值的**静态**关节角），
|
||||
# 于是新一轮的请求被 DDS 负载均衡到"残留的那个"，/joint_states 与 /tf 有两份
|
||||
# 来源互相盖。现象极具误导性：控制器 feedback 与 reference 完全一致（控制器没坏），
|
||||
# 但 TF 显示末端**一动不动** —— 顺着现象查会一路查到 IK / TF / 时钟上。
|
||||
#
|
||||
# 所以这里做三件事：
|
||||
# 1. run_real.sh 与 stop_real.sh 共用同一份扫描实现（不再各写一份会漂移的表）；
|
||||
# 2. 扫描 = 点名表 + `install/` 前缀兜底 —— 凡命令行含 ``<仓库>/install/``
|
||||
# 的进程都算真机进程，以后再加新节点，不改这个文件也能被扫到；
|
||||
# 3. **按 ROS_DOMAIN_ID 区分真机与仿真**：仿真强制 77（见 sim_env.sh），
|
||||
# 真机默认 0。只按名字扫的话，停真机会顺手把正在跑的仿真的
|
||||
# robot_state_publisher 一起杀掉（两边同名、都在 conda/ROS 下）。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 之后 source）：
|
||||
# source "$(dirname "$0")/lib/real_procs.sh"
|
||||
# mapfile -t targets < <(real_proc_targets) # 只输出 PID（已排除自身/祖先/仿真）
|
||||
# real_proc_scan # "PID<TAB>域<TAB>命令行"
|
||||
# =============================================================================
|
||||
|
||||
# 仿真的域（sim_env.sh 强制那个值）。真机扫到它 = 那是仿真进程，不是残留。
|
||||
REAL_SIM_DOMAIN=77
|
||||
|
||||
_real_repo_root() {
|
||||
cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd
|
||||
}
|
||||
|
||||
# 兜底模式：真机安装空间。用绝对路径前缀，避免误伤 install_sim/（仿真那边）。
|
||||
# 注意 "install_sim/" **不含** 子串 "install/"（_sim 把它截断了），所以这一条
|
||||
# 天然不会扫到仿真进程。
|
||||
#
|
||||
# ⚠️ 路径要按 awk 正则转义：路径里可能有 . + - 之类字符，不转义的话
|
||||
# "." 会匹配任意字符（匹配面变大 = 误杀无关进程）。
|
||||
_real_install_dir() {
|
||||
printf '%s/install/' "$(_real_repo_root)"
|
||||
}
|
||||
|
||||
_real_regex_escape() {
|
||||
printf '%s' "$1" | sed -e 's/[][\.^$*+?(){}|\\]/\\&/g'
|
||||
}
|
||||
|
||||
# 点名表：列**不在 install/ 下**、但属于真机栈的进程。
|
||||
# 它们的可执行文件来自 conda 环境或 ROS 发行版（/opt/ros/humble/lib/...），
|
||||
# 命令行里没有仓库的 install/ 字样，兜底规则扫不到。
|
||||
#
|
||||
# ros2_control_node 控制器本体（它占着 RDK，RDK 对同一台臂是独占的）
|
||||
# controller_manager.*spawner 孤儿 spawner，会永远等一个不存在的 controller_manager
|
||||
# joint_state_publisher **mock 专用**节点；在真机上出现 = 上一轮 mock 栈的残留，
|
||||
# 它发的静态关节角会盖掉真实值
|
||||
# robot_state_publisher 发 TF 与 /robot_description；残留会让"改了描述却不生效"
|
||||
# static_transform_publisher 发 latched 的 /tf_static，残留会一直把旧外参按在新树上
|
||||
REAL_PROC_NAMED='ros2_control_node|controller_manager.*spawner|joint_state_publisher|robot_state_publisher|static_transform_publisher'
|
||||
|
||||
real_proc_patterns() {
|
||||
printf '%s|%s' "$REAL_PROC_NAMED" "$(_real_regex_escape "$(_real_install_dir)")"
|
||||
}
|
||||
|
||||
# 读某个进程的 ROS_DOMAIN_ID。读不到（僵尸/无权限/没设）输出空。
|
||||
# `|| true`：调用方可能是 `set -e` 的脚本（run_real.sh），而 tr/sed 在
|
||||
# /proc 读不到时会非零返回 —— 不兜住的话整个启动脚本会被这一行带停。
|
||||
real_proc_domain() {
|
||||
local pid="$1" v=""
|
||||
v="$(tr '\0' '\n' < "/proc/${pid}/environ" 2>/dev/null \
|
||||
| sed -n 's/^ROS_DOMAIN_ID=//p' | head -1)" || true
|
||||
printf '%s' "${v:-}"
|
||||
}
|
||||
|
||||
# 是不是仿真进程（域 == REAL_SIM_DOMAIN）。
|
||||
# 读不到 environ 时**不当**成仿真：宁可停真机时多杀一个"域未知但确实在
|
||||
# install/ 下"的进程，也不要漏掉真机残留（漏掉的代价见文件头）。
|
||||
real_proc_is_sim() {
|
||||
[ "$(real_proc_domain "$1")" = "${REAL_SIM_DOMAIN}" ]
|
||||
}
|
||||
|
||||
# 扫描：输出 "PID<TAB>域<TAB>命令行"。
|
||||
#
|
||||
# ⚠️ 两个必须踩过的坑（与 sim_procs.sh 同源，别再犯）：
|
||||
# 1. `pkill -f <模式>` 会匹配到自己所在的 shell（它的命令行里就含那个模式），
|
||||
# 表现成"命令立刻返回、什么都没杀"。
|
||||
# 2. 模式串用**环境变量**传给 awk，不用 `-v pat=...`：用了 -v 的话 awk 自己的
|
||||
# 命令行里就含整串模式，于是它匹配到自己，复查时多报一个幽灵 PID。
|
||||
real_proc_scan() {
|
||||
export PAT="$(real_proc_patterns)"
|
||||
export ME="${REAL_PROC_SELF:-$$}"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（如 $(...) 的 fork）
|
||||
if (args ~ /real_procs\.sh/) next # 本库自己（含 grep/sed 的读数）
|
||||
if (args ~ /stop_real\.sh/) next
|
||||
if (args ~ /run_real\.sh/) next
|
||||
if (args ~ /^\s*awk/) next
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
|
||||
# 排除自己与全部祖先，跳过仿真进程，只输出 PID。
|
||||
# 设 REAL_PROC_INCLUDE_SIM=1 可以连仿真进程一起列出（自查用，停止流程不用）。
|
||||
real_proc_targets() {
|
||||
local exclude=() p=$$ pid rest _d skip e
|
||||
while [ "${p:-0}" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
while IFS=$'\t' read -r pid rest; do
|
||||
[ -z "${pid:-}" ] && continue
|
||||
skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
if [ "${REAL_PROC_INCLUDE_SIM:-0}" != "1" ] && real_proc_is_sim "$pid"; then
|
||||
continue
|
||||
fi
|
||||
echo "$pid"
|
||||
done < <(real_proc_scan)
|
||||
}
|
||||
Executable
+48
@@ -0,0 +1,48 @@
|
||||
#!/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）。
|
||||
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
|
||||
fi
|
||||
}
|
||||
Reference in New Issue
Block a user