Initial import of ORISYS arm workstation
This commit is contained in:
cheng
Executable
+42
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env bash
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
if [[ -f /opt/ros/jazzy/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
elif [[ -f /opt/ros/humble/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash
|
||||
elif [[ -f /opt/ros/iron/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/iron/setup.bash
|
||||
else
|
||||
echo "未检测到系统 ROS 2；使用 conda ros_flexiv 环境"
|
||||
fi
|
||||
fi
|
||||
|
||||
PACKAGES=(flexiv_description arm_msgs arm_description arm_kinematics arm_interfaces arm_control arm_driver_flexiv arm_bringup arm_application)
|
||||
# conda GCC 15 把 Humble 消息头里缺的 cstdint 当成错误。补上这个头，仍用 conda 编译器，
|
||||
# 才能和 ros_flexiv 里的 librclcpp 链在一起。
|
||||
export CXXFLAGS="${CXXFLAGS:-} -include cstdint -Wno-template-body"
|
||||
colcon build --symlink-install --packages-select "${PACKAGES[@]}" "$@"
|
||||
# shellcheck source=lib/fix_shebangs.sh
|
||||
source "${ROOT}/scripts/lib/fix_shebangs.sh"
|
||||
fix_shebangs "${ROOT}/install" "${CONDA_PREFIX:-/usr}/bin/python3"
|
||||
|
||||
echo "构建完成。请执行: source ${ROOT}/install/setup.bash"
|
||||
Executable
+33
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env bash
|
||||
# 构建整个工作区：机械臂栈 + Flexiv 真机驱动
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
|
||||
fi
|
||||
|
||||
echo "=== [1/2] arm stack ==="
|
||||
bash "${ROOT}/scripts/build.sh"
|
||||
|
||||
echo "=== [2/2] flexiv real stack ==="
|
||||
bash "${ROOT}/scripts/build_real.sh"
|
||||
|
||||
echo ""
|
||||
echo "Done. Use:"
|
||||
echo " source ${ROOT}/scripts/env.sh"
|
||||
echo " bash ${ROOT}/scripts/run_real.sh"
|
||||
Executable
+40
@@ -0,0 +1,40 @@
|
||||
#!/usr/bin/env bash
|
||||
# 构建真机用的 Flexiv 硬件包（前提：RDK 已装到 ~/flexiv_install）
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
|
||||
fi
|
||||
|
||||
PREFIX="${HOME}/flexiv_install"
|
||||
CMAKE_PREFIX="${PREFIX}"
|
||||
if [[ -n "${CONDA_PREFIX:-}" ]]; then
|
||||
CMAKE_PREFIX="${PREFIX};${CONDA_PREFIX}"
|
||||
fi
|
||||
|
||||
export CXXFLAGS="${CXXFLAGS:-} -include cstdint -Wno-template-body"
|
||||
colcon build --symlink-install --packages-select \
|
||||
flexiv_hardware \
|
||||
flexiv_msgs \
|
||||
flexiv_gripper \
|
||||
flexiv_test_nodes \
|
||||
flexiv_robot_states_broadcaster \
|
||||
flexiv_bringup \
|
||||
--cmake-args "-DCMAKE_PREFIX_PATH=${CMAKE_PREFIX}" "$@"
|
||||
|
||||
echo "Real stack build done. source ${ROOT}/scripts/env.sh"
|
||||
Executable
+64
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env bash
|
||||
# 加载本工作区环境（conda + ROS + flexiv 库）。
|
||||
# 用法：source scripts/env.sh
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
|
||||
if [[ -f /opt/ros/humble/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash
|
||||
elif [[ -f /opt/ros/jazzy/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
fi
|
||||
|
||||
# conda 可能在 ~/anaconda3 或 ~/miniconda3。不写死用户名。
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
dev_source_conda || true
|
||||
if command -v conda >/dev/null 2>&1; then
|
||||
# 干净切换：先把堆叠的激活退到 base，再激活真机环境。
|
||||
# 同一 shell 里从仿真环境切过来时，不清干净会残留 PATH/PYTHONPATH。
|
||||
_env_helper="${ROOT}/scripts/lib/conda_switch.sh"
|
||||
_real_env="${REAL_ENV_NAME:-ros_flexiv}"
|
||||
if [[ -f "${_env_helper}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${_env_helper}"
|
||||
conda_switch_clean "${_real_env}" "${SIM_ENV_NAME:-ros_gz_sim}" || \
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
else
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
fi
|
||||
unset _env_helper _real_env
|
||||
fi
|
||||
|
||||
if [[ -f "${ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/install/setup.bash"
|
||||
# 本机 /opt/ros/humble 会盖住 conda 的 ros2，3.10 解释器加载不了 3.11 的 rclpy。
|
||||
if [[ -n "${CONDA_PREFIX:-}" && -x "${CONDA_PREFIX}/bin/ros2" ]]; then
|
||||
export PATH="${CONDA_PREFIX}/bin:${PATH}"
|
||||
fi
|
||||
else
|
||||
echo "警告：${ROOT}/install 还没编译。工位窗口可以先开；真机栈要先在仓库根目录 colcon build（不要在中文路径下编）。" >&2
|
||||
fi
|
||||
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
|
||||
# pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它
|
||||
_pyver="$(python3 -c 'import sys; print(f"{sys.version_info[0]}.{sys.version_info[1]}")' 2>/dev/null || echo 3.11)"
|
||||
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/${REAL_ENV_NAME:-ros_flexiv}}/lib/python${_pyver}/site-packages"
|
||||
for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
|
||||
if [[ -d "${_d}" ]]; then
|
||||
export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}"
|
||||
fi
|
||||
done
|
||||
unset _d _TRT_SITE _pyver
|
||||
export YOLO_AUTOINSTALL=false
|
||||
export ROS2_WS="${ROOT}"
|
||||
|
||||
# 真机与仿真的 DDS 隔离（细节与踩坑见 lib/ros_domain.sh 的文件头）。
|
||||
# shellcheck source=lib/ros_domain.sh
|
||||
source "${ROOT}/scripts/lib/ros_domain.sh"
|
||||
ros_domain_force_real
|
||||
|
||||
# 主站点配置 → `SITE_*`（YOLO 权重、相机、机器人 SN……）
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
||||
@@ -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
|
||||
}
|
||||
Executable
+567
@@ -0,0 +1,567 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RDK 控制桥（必须在 rdk_py310 / Python 3.10 下运行）。
|
||||
|
||||
为什么需要这个进程
|
||||
------------------
|
||||
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对
|
||||
同一台机器人是**独占访问**：多个进程同时连同一个 SN 会失败。所以框架里所有
|
||||
RDK 操作都收敛到本进程，由它串行化，业务节点通过 JSON 行协议远程调用。
|
||||
|
||||
这也让 `arm_driver_flexiv` 不必 import flexivrdk，符合
|
||||
`docs/INTERFACES.md` 的约束（业务节点不得 import flexivrdk）。
|
||||
|
||||
软拖拽为什么用 NRT_JOINT_IMPEDANCE
|
||||
----------------------------------
|
||||
AUTO(Remote) 模式下示教器上的 Free Drive 按钮不可用（控制权在 Remote）。
|
||||
RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
|
||||
所以默认用低刚度 NRT_JOINT_IMPEDANCE：目标跟踪实测关节角 + 低刚度，
|
||||
于是手臂可以用手推动，同时 Remote 保持开启。
|
||||
|
||||
线程安全
|
||||
--------
|
||||
`flexivrdk.Robot` 不是线程安全的。所有 robot API 调用（含拖拽循环）共用同一
|
||||
把锁 `rlock`。拖拽期间的关节读取走缓存，避免 Enter 与 Stop() 竞争。
|
||||
|
||||
行协议（JSON 只写 stdout；RDK 自身日志走 stderr）
|
||||
-------------------------------------------------
|
||||
{"cmd":"ping"}
|
||||
{"cmd":"connect","sn":"<site.yaml 的 robot.sn>","iface":"<robot.iface>"}
|
||||
{"cmd":"enable"} # 使能 + 清故障（幂等）
|
||||
{"cmd":"clear_fault"} # 显式清故障并使能
|
||||
{"cmd":"switch_tool","name":"tool1"} # 切工具；name 空=只查询
|
||||
{"cmd":"status"} # operational/fault/mode/tool
|
||||
{"cmd":"drag_on","mode":"soft"} # soft | floating
|
||||
{"cmd":"drag_off"}
|
||||
{"cmd":"joints"}
|
||||
{"cmd":"move_joints","q":[0,0,0,0,0,0,0],"speed":0.15}
|
||||
{"cmd":"quit"}
|
||||
|
||||
每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
# 公共 RDK 操作统一在 scripts/lib/rdk_helpers.py，避免与 rdk_switch_tool.py
|
||||
# 各写一份导致行为漂移。
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent / 'lib'))
|
||||
from rdk_helpers import ( # noqa: E402
|
||||
clear_fault as rdk_clear_fault,
|
||||
ensure_idle,
|
||||
ensure_operational,
|
||||
rdk_whitelist_lists,
|
||||
resolve_tool_name,
|
||||
single_arm_groups,
|
||||
switch_tool,
|
||||
tool_name,
|
||||
tool_params,
|
||||
)
|
||||
|
||||
_JSON_OUT = sys.stdout
|
||||
sys.stdout = sys.stderr
|
||||
|
||||
|
||||
def reply(obj: dict) -> None:
|
||||
_JSON_OUT.write(json.dumps(obj, ensure_ascii=False) + '\n')
|
||||
_JSON_OUT.flush()
|
||||
|
||||
|
||||
def log(msg: str) -> None:
|
||||
print(f'[rdk_bridge] {msg}', file=sys.stderr, flush=True)
|
||||
|
||||
|
||||
class SoftDrag:
|
||||
def __init__(self, robot, flexivrdk, lock: threading.Lock) -> None:
|
||||
self.robot = robot
|
||||
self.flexivrdk = flexivrdk
|
||||
self._lock = lock
|
||||
self._stop = threading.Event()
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self.latest_q: List[float] = []
|
||||
self._groups = {}
|
||||
|
||||
def start(self) -> None:
|
||||
mode = self.flexivrdk.Mode
|
||||
robot = self.robot
|
||||
with self._lock:
|
||||
robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
|
||||
st = robot.states()
|
||||
self.latest_q = [float(v) for v in list(st.q)[:7]]
|
||||
try:
|
||||
info = robot.info()
|
||||
self._groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
except Exception as exc:
|
||||
log(f'info.single_arm_groups: {exc}')
|
||||
self._groups = {}
|
||||
if self._groups:
|
||||
for g in self._groups:
|
||||
try:
|
||||
nom = list(info.K_q_nom[g])
|
||||
soft = [max(8.0, 0.08 * float(v)) for v in nom]
|
||||
robot.SetJointImpedance(g, soft)
|
||||
log(f'SetJointImpedance {g} K={soft}')
|
||||
except Exception as exc:
|
||||
log(f'SetJointImpedance 组 {g}: {exc}')
|
||||
else:
|
||||
n = max(7, len(self.latest_q))
|
||||
try:
|
||||
robot.SetJointImpedance([35.0] * n, [0.7] * n)
|
||||
except Exception as exc:
|
||||
log(f'SetJointImpedance flat（不分组）: {exc}')
|
||||
|
||||
self._stop.clear()
|
||||
self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _loop(self) -> None:
|
||||
robot = self.robot
|
||||
flexivrdk = self.flexivrdk
|
||||
while not self._stop.is_set():
|
||||
try:
|
||||
with self._lock:
|
||||
if self._stop.is_set():
|
||||
break
|
||||
st = robot.states()
|
||||
q = [float(v) for v in list(st.q)[:7]]
|
||||
self.latest_q = q
|
||||
if self._groups:
|
||||
cmds = {}
|
||||
for g in self._groups:
|
||||
try:
|
||||
gq = list(st[g].q)
|
||||
except Exception:
|
||||
gq = q
|
||||
n = len(gq)
|
||||
cmds[g] = flexivrdk.NrtJointPositionCmd(
|
||||
gq, [0.0] * n, [1.5] * n, [2.0] * n
|
||||
)
|
||||
robot.SendJointPosition(cmds)
|
||||
else:
|
||||
n = len(q)
|
||||
robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
except Exception as exc:
|
||||
log(f'soft 循环: {exc}')
|
||||
time.sleep(0.15)
|
||||
# 约 20 Hz —— 大部分时间让锁保持释放状态
|
||||
for _ in range(10):
|
||||
if self._stop.is_set():
|
||||
return
|
||||
time.sleep(0.005)
|
||||
|
||||
def stop(self) -> None:
|
||||
self._stop.set()
|
||||
th = self._thread
|
||||
if th is not None:
|
||||
th.join(timeout=4.0)
|
||||
if th.is_alive():
|
||||
log('join 之后 soft 循环仍然存活（无论如何都会切到 IDLE）')
|
||||
self._thread = None
|
||||
|
||||
|
||||
def _ensure_operational(robot) -> Optional[str]:
|
||||
"""保留旧名以兼容既有调用点；实现已统一到 rdk_helpers.ensure_operational。"""
|
||||
return ensure_operational(robot)
|
||||
|
||||
|
||||
def _restore_tracking_impedance(robot, flexivrdk) -> None:
|
||||
"""把关节阻抗从软拖拽拉回名义值，否则回放点位时臂跟不住指令。"""
|
||||
try:
|
||||
info = robot.info()
|
||||
groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
except Exception as exc:
|
||||
log(f'读名义阻抗失败: {exc}')
|
||||
groups = {}
|
||||
if groups:
|
||||
for group in groups:
|
||||
try:
|
||||
nominal = [float(v) for v in list(info.K_q_nom[group])]
|
||||
robot.SetJointImpedance(group, nominal)
|
||||
log(f'恢复阻抗 {group} K={nominal}')
|
||||
except Exception as exc:
|
||||
log(f'恢复阻抗 {group}: {exc}')
|
||||
return
|
||||
try:
|
||||
robot.SetJointImpedance([350.0] * 7, [7.0] * 7)
|
||||
except Exception as exc:
|
||||
log(f'恢复阻抗: {exc}')
|
||||
|
||||
|
||||
def _send_joint_target(robot, flexivrdk, target: List[float], grouped: Optional[bool]) -> bool:
|
||||
"""下发 7 关节目标。返回是否走了分组接口。"""
|
||||
if grouped is True:
|
||||
_send_grouped_joint_target(robot, flexivrdk, target)
|
||||
return True
|
||||
try:
|
||||
n = len(target)
|
||||
robot.SendJointPosition(target, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
return False
|
||||
except Exception as exc:
|
||||
if grouped is False:
|
||||
raise
|
||||
log(f'扁平关节指令失败，改分组: {exc}')
|
||||
_send_grouped_joint_target(robot, flexivrdk, target)
|
||||
return True
|
||||
|
||||
|
||||
def _send_grouped_joint_target(robot, flexivrdk, target: List[float]) -> None:
|
||||
info = robot.info()
|
||||
groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
if not groups:
|
||||
raise RuntimeError('没有分组，扁平关节指令也失败了')
|
||||
states = robot.states()
|
||||
cmds = {}
|
||||
placed = False
|
||||
for group in groups:
|
||||
try:
|
||||
current = [float(v) for v in list(states[group].q)]
|
||||
except Exception:
|
||||
current = list(target)
|
||||
use = target if (not placed and len(current) == len(target)) else current
|
||||
if use is target:
|
||||
placed = True
|
||||
n = len(use)
|
||||
cmds[group] = flexivrdk.NrtJointPositionCmd(use, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
if not placed:
|
||||
raise RuntimeError('没有一组关节数和示教点一致，拒绝下发')
|
||||
robot.SendJointPosition(cmds)
|
||||
|
||||
|
||||
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
|
||||
"""切到 IDLE，同时避免把 stdin 循环永久阻塞住。"""
|
||||
done = threading.Event()
|
||||
|
||||
def run() -> None:
|
||||
try:
|
||||
acquired = lock.acquire(timeout=2.0)
|
||||
try:
|
||||
if acquired:
|
||||
try:
|
||||
robot.Stop()
|
||||
except Exception as exc:
|
||||
log(f'Stop: {exc}')
|
||||
try:
|
||||
robot.SwitchMode(flexivrdk.Mode.IDLE)
|
||||
except Exception as exc:
|
||||
log(f'SwitchMode IDLE: {exc}')
|
||||
finally:
|
||||
if acquired:
|
||||
lock.release()
|
||||
finally:
|
||||
done.set()
|
||||
|
||||
threading.Thread(target=run, name='idle', daemon=True).start()
|
||||
ok = done.wait(timeout)
|
||||
if not ok:
|
||||
log('IDLE/Stop 超时 —— stdin 循环继续运行')
|
||||
return ok
|
||||
|
||||
|
||||
def main() -> int:
|
||||
import flexivrdk
|
||||
|
||||
robot = None
|
||||
drag_on = False
|
||||
soft: Optional[SoftDrag] = None
|
||||
rlock = threading.Lock()
|
||||
cached_q: List[float] = []
|
||||
|
||||
for line in sys.stdin:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
msg = json.loads(line)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
reply({'ok': False, 'error': f'JSON 解析失败: {exc}'})
|
||||
continue
|
||||
|
||||
cmd = str(msg.get('cmd', '')).strip()
|
||||
try:
|
||||
if cmd == 'ping':
|
||||
reply({'ok': True, 'py': sys.version.split()[0]})
|
||||
|
||||
elif cmd == 'connect':
|
||||
sn = str(msg.get('sn') or os.environ.get('SITE_ROBOT_SN') or '')
|
||||
iface = str(msg.get('iface') or '')
|
||||
robot = None
|
||||
last_err = None
|
||||
attempts = []
|
||||
# 白名单必须是 IPv4。传 eth0 / enx… 会被 FastDDS 整段滤掉，
|
||||
# ping 通也会 Failed to connect。
|
||||
for listen in rdk_whitelist_lists(iface):
|
||||
log('白名单: ' + ', '.join(listen))
|
||||
attempts.extend([(sn, listen), (sn, listen, True)])
|
||||
attempts.append((sn,))
|
||||
for args in attempts:
|
||||
try:
|
||||
robot = flexivrdk.Robot(*args)
|
||||
log(f'已连接 {sn} args={args!r}')
|
||||
last_err = None
|
||||
break
|
||||
except TypeError:
|
||||
continue
|
||||
except Exception as exc:
|
||||
last_err = exc
|
||||
log(f'连接 {args!r}: {exc}')
|
||||
robot = None
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': str(last_err or '连接失败')})
|
||||
continue
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
with rlock:
|
||||
q = [float(v) for v in list(robot.states().q)[:7]]
|
||||
cached_q = q
|
||||
reply({'ok': True, 'sn': sn, 'q': q})
|
||||
|
||||
elif cmd == 'drag_on':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
|
||||
prefer = str(msg.get('mode', 'soft')).strip().lower()
|
||||
if prefer in ('floating', 'primitive', 'floatingjoint'):
|
||||
name = str(msg.get('primitive', 'FloatingJoint')).replace('()', '')
|
||||
try:
|
||||
with rlock:
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
|
||||
robot.ExecutePrimitive(name, {})
|
||||
drag_on = True
|
||||
reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name})
|
||||
continue
|
||||
except Exception as exc:
|
||||
log(f'Floating 失败，退回 soft 模式: {exc}')
|
||||
|
||||
try:
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = SoftDrag(robot, flexivrdk, rlock)
|
||||
soft.start()
|
||||
if soft.latest_q:
|
||||
cached_q = list(soft.latest_q)
|
||||
drag_on = True
|
||||
log('软拖拽已开启')
|
||||
reply({
|
||||
'ok': True,
|
||||
'drag': True,
|
||||
'mode': 'soft',
|
||||
'hint': '轻推机械臂即可，关节阻抗已调低。Enter=拍照（s=保持）。',
|
||||
})
|
||||
except Exception as exc:
|
||||
reply({'ok': False, 'error': f'软拖拽失败: {exc}'})
|
||||
|
||||
elif cmd == 'drag_off':
|
||||
log('收到 drag_off 请求')
|
||||
if soft is not None:
|
||||
if soft.latest_q:
|
||||
cached_q = list(soft.latest_q)
|
||||
soft.stop()
|
||||
soft = None
|
||||
if robot is not None:
|
||||
_idle_async(robot, rlock, flexivrdk, timeout=4.0)
|
||||
drag_on = False
|
||||
log('drag_off 已完成')
|
||||
reply({'ok': True, 'drag': False, 'q': cached_q})
|
||||
|
||||
elif cmd == 'joints':
|
||||
q: Optional[List[float]] = None
|
||||
if soft is not None and len(soft.latest_q) >= 7:
|
||||
q = list(soft.latest_q)
|
||||
elif len(cached_q) >= 7:
|
||||
q = list(cached_q)
|
||||
if q is None or len(q) < 7:
|
||||
reply({'ok': False, 'error': '没有关节缓存'})
|
||||
continue
|
||||
cached_q = q
|
||||
reply({'ok': True, 'q': q, 'drag': drag_on})
|
||||
|
||||
elif cmd == 'enable':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = ensure_operational(robot)
|
||||
reply({
|
||||
'ok': err is None,
|
||||
'error': err,
|
||||
'operational': bool(robot.operational()),
|
||||
})
|
||||
|
||||
elif cmd == 'clear_fault':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = rdk_clear_fault(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
# 清故障后顺手使能，否则调用方还得再发一条 enable。
|
||||
err2 = ensure_operational(robot)
|
||||
reply({
|
||||
'ok': err2 is None,
|
||||
'error': err2,
|
||||
'fault': bool(robot.fault()),
|
||||
'operational': bool(robot.operational()),
|
||||
})
|
||||
|
||||
elif cmd == 'switch_tool':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
want = str(msg.get('name', '')).strip()
|
||||
|
||||
# 切换工具要求 IDLE；这一步是同步的，因为切换本身不能被打断。
|
||||
err = ensure_idle(robot, flexivrdk.Mode, timeout_s=12.0)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
|
||||
try:
|
||||
with rlock:
|
||||
tool = flexivrdk.Tool(robot)
|
||||
groups = single_arm_groups(robot)
|
||||
available = [str(n) for n in (tool.list() or [])]
|
||||
anchor = groups[0] if groups else None
|
||||
|
||||
if not want:
|
||||
# 纯查询：不切换，把当前工具与参数报回去。
|
||||
params = tool_params(tool, anchor)
|
||||
reply({
|
||||
'ok': True,
|
||||
'switched': False,
|
||||
'current': tool_name(tool, anchor),
|
||||
'available': available,
|
||||
**params,
|
||||
})
|
||||
continue
|
||||
|
||||
target = resolve_tool_name(tool, want)
|
||||
already = all(tool_name(tool, g) == target for g in groups)
|
||||
if not already:
|
||||
for g in groups:
|
||||
switch_tool(tool, g, target)
|
||||
time.sleep(0.3)
|
||||
|
||||
params = tool_params(tool, anchor)
|
||||
reply({
|
||||
'ok': True,
|
||||
'switched': not already,
|
||||
'already': already,
|
||||
'current': tool_name(tool, anchor),
|
||||
'target': target,
|
||||
'available': available,
|
||||
**params,
|
||||
})
|
||||
except Exception as exc: # noqa: BLE001
|
||||
reply({'ok': False, 'error': str(exc)})
|
||||
|
||||
elif cmd == 'status':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'connected': False, 'error': '尚未 connect'})
|
||||
continue
|
||||
info = {'ok': True, 'connected': True, 'drag': drag_on}
|
||||
try:
|
||||
info['operational'] = bool(robot.operational())
|
||||
info['fault'] = bool(robot.fault())
|
||||
info['mode'] = str(robot.mode())
|
||||
except Exception as exc: # noqa: BLE001
|
||||
info['error'] = str(exc)
|
||||
try:
|
||||
with rlock:
|
||||
tool = flexivrdk.Tool(robot)
|
||||
groups = single_arm_groups(robot)
|
||||
info['tool'] = tool_name(tool, groups[0] if groups else None)
|
||||
except Exception:
|
||||
info['tool'] = ''
|
||||
reply(info)
|
||||
|
||||
elif cmd == 'move_joints':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
raw = msg.get('q')
|
||||
if not isinstance(raw, list) or len(raw) < 7:
|
||||
reply({'ok': False, 'error': '需要 7 个关节角（弧度）'})
|
||||
continue
|
||||
try:
|
||||
target = [float(v) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
reply({'ok': False, 'error': '关节角不是数字'})
|
||||
continue
|
||||
if any(v != v or abs(v) > 12.6 for v in target):
|
||||
reply({'ok': False, 'error': '关节角不是有限值，或超出约两圈'})
|
||||
continue
|
||||
try:
|
||||
speed = float(msg.get('speed', 0.15))
|
||||
except (TypeError, ValueError):
|
||||
speed = 0.15
|
||||
speed = max(0.05, min(0.35, speed))
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = None
|
||||
drag_on = False
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
try:
|
||||
with rlock:
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_JOINT_IMPEDANCE)
|
||||
current = [float(v) for v in list(robot.states().q)[:7]]
|
||||
_restore_tracking_impedance(robot, flexivrdk)
|
||||
max_delta = max(abs(a - b) for a, b in zip(current, target))
|
||||
step_s = 0.05
|
||||
steps = max(1, int(max_delta / (speed * step_s)))
|
||||
if steps * step_s > 150.0:
|
||||
reply({
|
||||
'ok': False,
|
||||
'error': f'路程太长（{max_delta:.2f} rad），请先拖近一点再回放',
|
||||
})
|
||||
continue
|
||||
grouped: Optional[bool] = None
|
||||
for index in range(1, steps + 1):
|
||||
alpha = index / steps
|
||||
sample = [a + (b - a) * alpha for a, b in zip(current, target)]
|
||||
with rlock:
|
||||
grouped = _send_joint_target(robot, flexivrdk, sample, grouped)
|
||||
time.sleep(step_s)
|
||||
for _hold in range(8):
|
||||
with rlock:
|
||||
grouped = _send_joint_target(robot, flexivrdk, target, grouped)
|
||||
time.sleep(step_s)
|
||||
cached_q = list(target)
|
||||
reply({'ok': True, 'q': target, 'from': current, 'steps': steps, 'speed': speed})
|
||||
except Exception as exc:
|
||||
reply({'ok': False, 'error': f'回点失败: {exc}'})
|
||||
|
||||
elif cmd == 'quit':
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = None
|
||||
if robot is not None and drag_on:
|
||||
_idle_async(robot, rlock, flexivrdk, timeout=3.0)
|
||||
reply({'ok': True})
|
||||
return 0
|
||||
|
||||
else:
|
||||
reply({'ok': False, 'error': f'未知命令 {cmd}'})
|
||||
except Exception as exc: # noqa: BLE001
|
||||
log(f'cmd={cmd} 出错: {exc}')
|
||||
reply({'ok': False, 'error': str(exc)})
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
Executable
+276
@@ -0,0 +1,276 @@
|
||||
#!/usr/bin/env python3
|
||||
"""把 Flexiv 当前工具（TCP + 负载）切换成 Studio 里配置好的名字。
|
||||
|
||||
必须在 RDK 的 Python（rdk_py310）下运行。机器人必须处于 IDLE；如果
|
||||
ros2_control 已经占住了 RDK 连接，请先停掉 run_real.sh。
|
||||
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py # site.yaml robot.tool_name
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
|
||||
|
||||
def _single_arm_groups(robot):
|
||||
"""RDK 1.9 的 Tool API 是单臂的（没有 JointGroup）。老版本用 groups。"""
|
||||
info = robot.info()
|
||||
groups = getattr(info, "single_arm_groups", None) or {}
|
||||
if isinstance(groups, dict):
|
||||
keys = list(groups.keys())
|
||||
else:
|
||||
keys = list(groups)
|
||||
if keys:
|
||||
return keys
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
if hasattr(flexivrdk, "JointGroup"):
|
||||
return [flexivrdk.JointGroup.ARM_1]
|
||||
except Exception:
|
||||
pass
|
||||
return [None] # 哨兵：调用 Tool.name()/Switch() 时不带 group
|
||||
|
||||
|
||||
def _tool_name(tool, group):
|
||||
if group is None:
|
||||
return tool.name()
|
||||
return tool.name(group)
|
||||
|
||||
|
||||
def _tool_params(tool, group):
|
||||
if group is None:
|
||||
return tool.params()
|
||||
return tool.params(group)
|
||||
|
||||
|
||||
def _tool_switch(tool, group, target: str) -> None:
|
||||
if group is None:
|
||||
tool.Switch(target)
|
||||
return
|
||||
tool.Switch(group, target)
|
||||
|
||||
|
||||
def _group_name(group) -> str:
|
||||
if group is None:
|
||||
return "单臂"
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
return str(flexivrdk.kJointGroupNames[group])
|
||||
except Exception:
|
||||
return str(group)
|
||||
|
||||
|
||||
def _fmt_params(params) -> str:
|
||||
mass = getattr(params, "mass", None)
|
||||
com = list(getattr(params, "CoM", []) or [])
|
||||
tcp = list(getattr(params, "tcp_location", []) or [])
|
||||
com_s = ",".join(f"{float(v):.4f}" for v in com[:3]) if com else "?"
|
||||
tcp_s = ",".join(f"{float(v):.4f}" for v in tcp[:3]) if tcp else "?"
|
||||
return f"mass={mass} kg CoM=[{com_s}] m TCP_xyz=[{tcp_s}] m"
|
||||
|
||||
|
||||
def _ensure_idle(robot, mode) -> None:
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
if hasattr(robot, "Enable"):
|
||||
robot.Enable()
|
||||
else:
|
||||
robot.ServoOn()
|
||||
for _ in range(40):
|
||||
if robot.operational():
|
||||
break
|
||||
time.sleep(0.25)
|
||||
if not robot.operational():
|
||||
raise RuntimeError(
|
||||
"机器人未处于 operational 状态 —— 请在示教器切 AUTO Remote 并使能，松开急停"
|
||||
)
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
|
||||
|
||||
def _resolve_name(tool, want: str) -> str:
|
||||
want = (want or "").strip()
|
||||
if not want:
|
||||
return ""
|
||||
names = list(tool.list() or [])
|
||||
if want in names:
|
||||
return want
|
||||
lower = {n.lower(): n for n in names}
|
||||
hit = lower.get(want.lower())
|
||||
if hit:
|
||||
return hit
|
||||
raise SystemExit(
|
||||
"工具 %r 不在 Studio 工具池里。\n 可选: %s"
|
||||
% (want, ", ".join(names) or "（空）")
|
||||
)
|
||||
|
||||
|
||||
def _iface_ipv4s(name: str) -> list:
|
||||
"""FastDDS 白名单要的是 IPv4 地址，而不是 enp4s0 这类网卡名。"""
|
||||
import socket
|
||||
import fcntl
|
||||
import struct
|
||||
|
||||
name = (name or "").strip()
|
||||
if not name:
|
||||
return []
|
||||
if name.count(".") == 3:
|
||||
return [name]
|
||||
addrs = []
|
||||
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:
|
||||
import subprocess
|
||||
|
||||
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:
|
||||
cidr = parts[parts.index("inet") + 1]
|
||||
addrs.append(cidr.split("/")[0])
|
||||
except Exception:
|
||||
pass
|
||||
# 去重，保持原有顺序
|
||||
seen = set()
|
||||
uniq = []
|
||||
for a in addrs:
|
||||
if a and a not in seen:
|
||||
seen.add(a)
|
||||
uniq.append(a)
|
||||
return uniq
|
||||
|
||||
|
||||
def _rdk_whitelist(iface: str) -> list[list[str]]:
|
||||
"""只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
|
||||
sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
|
||||
from rdk_helpers import rdk_whitelist_lists # noqa: PLC0415
|
||||
|
||||
return rdk_whitelist_lists(iface)
|
||||
|
||||
|
||||
def connect_rdk(sn: str, iface: str = ""):
|
||||
import flexivrdk
|
||||
|
||||
for listen in _rdk_whitelist(iface):
|
||||
print("RDK 白名单:", ", ".join(listen))
|
||||
for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)):
|
||||
try:
|
||||
return flexivrdk.Robot(*args)
|
||||
except TypeError:
|
||||
continue
|
||||
except RuntimeError as exc:
|
||||
msg = str(exc)
|
||||
if "whitelist" in msg.lower() or "filtered out" in msg.lower():
|
||||
print(" 白名单被拒，试下一个")
|
||||
break
|
||||
if "Failed to connect" in msg or "failed to connect" in msg.lower():
|
||||
print(" 用这个白名单搜索超时")
|
||||
break
|
||||
raise
|
||||
print("不带 iface 过滤再试一次")
|
||||
return flexivrdk.Robot(sn)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="切换 Flexiv Studio 工具 / TCP")
|
||||
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml）")
|
||||
ap.add_argument("--iface", default="", help="RDK 用的网卡白名单（如 enp4s0）")
|
||||
ap.add_argument(
|
||||
"--name",
|
||||
default="",
|
||||
help="Studio 工具名（Flange = 不装工具）。留空 = 用 site.yaml 的 robot.tool_name",
|
||||
)
|
||||
ap.add_argument("--list", action="store_true", help="列出工具后退出")
|
||||
args = ap.parse_args()
|
||||
|
||||
sn = (args.sn or "").strip()
|
||||
want = (args.name or "").strip()
|
||||
if not sn:
|
||||
print("错误：必须提供 --sn（请通过 run_real.sh / rdk_switch_tool.sh 运行）", file=sys.stderr)
|
||||
return 2
|
||||
|
||||
import flexivrdk
|
||||
|
||||
mode = flexivrdk.Mode
|
||||
iface = (args.iface or "").strip()
|
||||
print(f"=== RDK 工具 SN={sn} iface={iface or 'auto'} ===")
|
||||
robot = None
|
||||
try:
|
||||
robot = connect_rdk(sn, iface)
|
||||
if robot.fault():
|
||||
print("检测到故障，正在 ClearFault ...")
|
||||
if not robot.ClearFault():
|
||||
print("错误：故障无法清除", file=sys.stderr)
|
||||
return 1
|
||||
tool = flexivrdk.Tool(robot)
|
||||
names = list(tool.list() or [])
|
||||
groups = _single_arm_groups(robot)
|
||||
print("Studio 工具:", ", ".join(names) or "（无）")
|
||||
for g in groups:
|
||||
cur = _tool_name(tool, g)
|
||||
extra = ""
|
||||
try:
|
||||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||||
except Exception:
|
||||
pass
|
||||
print(f" 当前 [{_group_name(g)}]: {cur}{extra}")
|
||||
|
||||
if args.list:
|
||||
return 0
|
||||
if not want:
|
||||
print("tool_name 为空 —— 保持当前工具（要切换请在 site.yaml 里设置 robot.tool_name）")
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
|
||||
target = _resolve_name(tool, want)
|
||||
already = all(_tool_name(tool, g) == target for g in groups)
|
||||
if already:
|
||||
print(f"已经处于 [{target}]")
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
|
||||
print(f"正在切换到 → [{target}]（需要 IDLE）")
|
||||
_ensure_idle(robot, mode)
|
||||
for g in groups:
|
||||
_tool_switch(tool, g, target)
|
||||
for g in groups:
|
||||
cur = _tool_name(tool, g)
|
||||
extra = ""
|
||||
try:
|
||||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||||
except Exception:
|
||||
pass
|
||||
print(f" 现在 [{_group_name(g)}]: {cur}{extra}")
|
||||
if cur != target:
|
||||
print(f"错误：切换失败，当前仍是 {cur!r}", file=sys.stderr)
|
||||
return 1
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
print(" 请先停掉 run_real.sh（RDK 是独占的）。", file=sys.stderr)
|
||||
print(" 示教器：切 AUTO Remote，并使能 Ethernet/RDK。", file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
robot = None
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Executable
+288
@@ -0,0 +1,288 @@
|
||||
#!/usr/bin/env bash
|
||||
# 跟踪 → 对孔。搜孔轻压已取消，本脚本不再进入那一步，也不能用参数打开。
|
||||
#
|
||||
# 为什么要单独有这个脚本：
|
||||
# pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
|
||||
# 它**不**拉相机、也**不**拉触觉（见它文件头的注释）。前置的相机与触感服务
|
||||
# 必须由本脚本拉起一次，否则二/三阶段会因为拿不到图像或力数据而空转。
|
||||
#
|
||||
# 本脚本做的事（顺序固定）：
|
||||
# 1) 确认机械臂控制器已 ACTIVE（不在就直接报错退出，不假装能跑）
|
||||
# 2) 拉起 D405 相机（已在发图就跳过）
|
||||
# 3) 拉起触觉服务（只有左臂装了传感器；右臂自动跳过）
|
||||
# 4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 指定对 k2 孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
|
||||
# 旋入不在这里。用 run_stage3_search.sh --arm 89 --screw
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 # 启动后先到这个法兰高度 [m]
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34 # 对孔悬停法兰高度 [m]，不安全会拒绝
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 下臂锁 XY，规尖停在离面 8mm（site 默认 2）
|
||||
#
|
||||
# 前置（另开一个终端，先跑起来）：
|
||||
# bash ~/ros2/scripts/run_real.sh --arm 89
|
||||
#
|
||||
# 注意：run_pipeline.sh 是本脚本的别名（历史名字），两者等价。
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
|
||||
# ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
|
||||
SKIP_STAGE1=false
|
||||
SKIP_STAGE2=false
|
||||
SKIP_STAGE3=true # 搜孔已取消。--skip-stage3 仍然接受，但没有开关能把它重新打开。
|
||||
HOLE=""
|
||||
DEPTH=""
|
||||
PITCH=""
|
||||
PIPELINE_PREVIEW=true # 传给 pipeline 节点：是否显示推理预览
|
||||
RESNAP=false
|
||||
WITH_CAMERA=true
|
||||
WITH_TACTILE=true
|
||||
CAMERA_PREVIEW=false
|
||||
NO_DESCENT=false
|
||||
INIT_Z=""
|
||||
HOVER_Z=""
|
||||
FINAL_GAP=""
|
||||
|
||||
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--skip-stage1) SKIP_STAGE1=true; shift ;;
|
||||
--skip-stage2) SKIP_STAGE2=true; shift ;;
|
||||
--skip-stage3) SKIP_STAGE3=true; shift ;;
|
||||
--hole) HOLE="${2:?--hole 需要一个孔名（k1 或 k2）}"; shift 2 ;;
|
||||
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
|
||||
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
|
||||
--resnap) RESNAP=true; shift ;;
|
||||
# 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
|
||||
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
|
||||
--no-descent) NO_DESCENT=true; shift ;;
|
||||
--init-z) INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--hover-z) HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
|
||||
--no-preview) PIPELINE_PREVIEW=false; shift ;;
|
||||
--camera-preview) CAMERA_PREVIEW=true; shift ;;
|
||||
--no-camera) WITH_CAMERA=false; shift ;;
|
||||
--no-tactile) WITH_TACTILE=false; shift ;;
|
||||
-h|--help) sed -n '2,32p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
|
||||
echo "ERROR: 搜孔轻压已取消，整体流程不再搜孔，也不在这里旋入。" >&2
|
||||
echo " 旋入请用: bash ~/ros2/scripts/run_stage3_search.sh --arm ${SITE_ROBOT_ARM:-89} --screw" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
|
||||
echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
|
||||
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
|
||||
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
|
||||
fi
|
||||
|
||||
echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
|
||||
|
||||
# ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
|
||||
echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
|
||||
# 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满
|
||||
# 超时才返回，30 次循环能白等两三分钟（实测 63s），用户会以为脚本卡死了。
|
||||
ready=false
|
||||
if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
|
||||
for _i in $(seq 1 20); do
|
||||
if timeout 3 ros2 service call /controller_manager/list_controllers \
|
||||
controller_manager_msgs/srv/ListControllers 2>/dev/null \
|
||||
| grep -q "name='rizon_arm_controller', state='active'"; then
|
||||
ready=true
|
||||
break
|
||||
fi
|
||||
sleep 0.5
|
||||
done
|
||||
fi
|
||||
if [[ "${ready}" != true ]]; then
|
||||
echo "ERROR: 机械臂控制器未激活。请先在另一终端跑：" >&2
|
||||
echo " bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
|
||||
echo "（若该终端已在跑，先确认它没报错退出、且 Rizon 上电使能成功。）" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " rizon_arm_controller ACTIVE"
|
||||
|
||||
# ---- 2) 相机。已在发图就跳过（避免重复起导致 USB 独占失败）----
|
||||
CAM_NS="${SITE_CAM_NAME:-d405_left}"
|
||||
IMG_TOPIC="/${CAM_NS}/color/image_raw"
|
||||
|
||||
_camera_ok() {
|
||||
# 以真实消息为准：topic info 的 Publisher count 依赖 DDS 发现，
|
||||
# 进程频繁重启后会出现假阴性（run_stage2_locate.sh 里踩过这个坑）。
|
||||
# 但先查 topic 列表，避免话题根本不存在时 echo 白等到超时。
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${IMG_TOPIC}" || return 1
|
||||
timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||||
}
|
||||
|
||||
if [[ "${WITH_CAMERA}" == true ]]; then
|
||||
if _camera_ok; then
|
||||
echo "=== 相机 ${IMG_TOPIC} 已在发图，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起相机 ${CAM_NS} ==="
|
||||
# 后台拉起；默认不带预览窗（nohup 下开 Qt 窗口没有意义）。
|
||||
# 想实时看画面就用 --camera-preview，或干脆另开终端跑 run_camera_d405.sh --preview。
|
||||
CAM_ARGS=()
|
||||
[[ "${CAMERA_PREVIEW}" == true ]] && CAM_ARGS+=(--preview) || CAM_ARGS+=(--no-preview)
|
||||
nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
|
||||
"${CAM_ARGS[@]}" > /tmp/run_all_camera.log 2>&1 &
|
||||
echo " camera pid=$! log=/tmp/run_all_camera.log"
|
||||
cam_ok=false
|
||||
for _i in $(seq 1 24); do # 最多等 ~12s；RealSense 枚举 + 首帧比想象中慢
|
||||
if _camera_ok; then cam_ok=true; break; fi
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${cam_ok}" != true ]]; then
|
||||
echo "ERROR: ${IMG_TOPIC} 仍无图像。看日志：/tmp/run_all_camera.log" >&2
|
||||
echo " 常见原因：USB 没插好；或另一个进程占着相机（USB 是独占的）。" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " 相机 ${CAM_NS} 出图 OK"
|
||||
fi
|
||||
# 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。
|
||||
D435_TOPIC="/d435/color/image_raw"
|
||||
_d435_ok() {
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
|
||||
timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||||
}
|
||||
if _d435_ok; then
|
||||
echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起顶视 D435（一阶段一次性定位）==="
|
||||
D435_ARGS=()
|
||||
[[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview)
|
||||
nohup bash "${ROOT}/scripts/run_camera_d435.sh" \
|
||||
"${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 &
|
||||
echo " d435 pid=$! log=/tmp/run_all_d435.log"
|
||||
d435_ok=false
|
||||
for _i in $(seq 1 24); do
|
||||
if _d435_ok; then d435_ok=true; break; fi
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${d435_ok}" != true ]]; then
|
||||
echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2
|
||||
echo " 混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " 顶视 D435 出图 OK"
|
||||
fi
|
||||
else
|
||||
echo "=== --no-camera：跳过相机检查 ==="
|
||||
fi
|
||||
|
||||
# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
|
||||
# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
|
||||
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
|
||||
_SKIP_TACTILE_REASON=""
|
||||
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
|
||||
echo "=== 搜孔已取消：跟踪和对孔不用触觉，跳过拉起 ==="
|
||||
WITH_TACTILE=false
|
||||
_SKIP_TACTILE_REASON="stage3"
|
||||
fi
|
||||
|
||||
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
|
||||
TACTILE_TOPIC="/tactile/left/status"
|
||||
_tactile_ok() {
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
|
||||
timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
|
||||
}
|
||||
|
||||
if [[ "${WITH_TACTILE}" == true ]]; then
|
||||
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算。
|
||||
# 以前这里写的是 `SN == Rizon4-063989`：换一台机器人就再也拉不起触觉服务，
|
||||
# 而且失败现象是"搜孔时力阈值全是 0"，离真正原因很远。
|
||||
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||||
if _tactile_ok; then
|
||||
echo "=== 触觉 ${TACTILE_TOPIC} 已在发，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起触觉服务（左臂）==="
|
||||
# 先清干净：上一次异常退出留下的 worker 会占死 /dev/video*，
|
||||
# 新进程会以"打不开相机"的形式失败，报错信息看不出根因。
|
||||
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
||||
pkill orisys_tac_w 2>/dev/null || true
|
||||
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
|
||||
sleep 1.0
|
||||
nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
|
||||
echo " tactile pid=$! log=/tmp/run_all_tactile.log"
|
||||
echo " 等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
|
||||
t_ok=false
|
||||
for _i in $(seq 1 8); do
|
||||
if _tactile_ok; then
|
||||
t_ok=true
|
||||
break
|
||||
fi
|
||||
# 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
|
||||
if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
|
||||
echo " 触觉 USB 打不开，停止等待" >&2
|
||||
break
|
||||
fi
|
||||
echo " 触觉还未 connected（${_i}/8）"
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${t_ok}" != true ]]; then
|
||||
# 触觉不通不阻断主流程：三阶段的搜孔轻压需要力数据，但一/二阶段不需要。
|
||||
# 让用户自己判断是先把传感器弄好，还是只跑到对孔就停。
|
||||
echo "WARN: 触觉话题仍未就绪（传感器没接？）。流程继续，三阶段搜孔会受影响。" >&2
|
||||
echo " 日志：/tmp/run_all_tactile.log" >&2
|
||||
else
|
||||
echo " 触觉就绪 OK"
|
||||
fi
|
||||
fi
|
||||
else
|
||||
echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
|
||||
fi
|
||||
elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
|
||||
echo "=== --no-tactile：跳过触觉检查 ==="
|
||||
fi
|
||||
|
||||
# ---- 4) 清掉可能打架的旧进程 ----
|
||||
echo "=== 清理旧的一阶段伺服/感知进程 ==="
|
||||
pkill -f 'visual_servo.launch' 2>/dev/null || true
|
||||
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
|
||||
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
|
||||
pkill -f 'lib/arm_application/assembly_pipeline' 2>/dev/null || true
|
||||
sleep 1
|
||||
|
||||
# ---- 5) exec pipeline.launch.py（把 CLI 映射成它的 launch 参数）----
|
||||
ARGS=(
|
||||
"skip_stage1:=${SKIP_STAGE1}"
|
||||
"skip_stage2:=${SKIP_STAGE2}"
|
||||
"skip_stage3:=${SKIP_STAGE3}"
|
||||
"preview:=${PIPELINE_PREVIEW}"
|
||||
"resnap:=${RESNAP}"
|
||||
"no_descent:=${NO_DESCENT}"
|
||||
"d435_move_only:=false"
|
||||
)
|
||||
[[ -n "${INIT_Z}" ]] && ARGS+=("init_z_m:=${INIT_Z}")
|
||||
[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
|
||||
[[ -n "${FINAL_GAP}" ]] && ARGS+=("final_gap_mm:=${FINAL_GAP}")
|
||||
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
|
||||
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
|
||||
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
|
||||
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
|
||||
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
|
||||
Executable
+166
@@ -0,0 +1,166 @@
|
||||
#!/usr/bin/env bash
|
||||
# 终端方式运行 D405：走官方 pyrealsense2 SDK —— 发布彩色 + 深度 + 双目 IR 话题。
|
||||
# 带 --preview 时，Qt 窗口跑在**独立的**查看器进程（`realsense_view`）里，
|
||||
# 这样绘制界面永远不会拖慢相机数据流。关掉窗口 ≠ 停止相机。
|
||||
# 不要同时启动 `view_realsense_depth.sh`（USB 是独占的）。
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh --preview
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
PREVIEW=-1 # -1 = 沿用 site；0/1 = 强制
|
||||
HIGH_FPS=0
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--preview) PREVIEW=1; shift ;;
|
||||
--no-preview) PREVIEW=0; shift ;;
|
||||
--30fps) HIGH_FPS=1; shift ;;
|
||||
-h|--help) sed -n '2,14p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
CAM_NAME="${SITE_CAM_NAME:-d405}"
|
||||
# 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view，
|
||||
# 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉），
|
||||
# 现场看起来就像"只剩眼外画面、绿框也不见了"。
|
||||
pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true
|
||||
sleep 2
|
||||
|
||||
USB_INFO="$(python3 - <<PY 2>/dev/null || true
|
||||
import pyrealsense2 as rs
|
||||
want_sn = "${SITE_CAM_SERIAL:-}"
|
||||
ds = list(rs.context().devices)
|
||||
if not ds:
|
||||
print("")
|
||||
raise SystemExit
|
||||
pick = None
|
||||
for d in ds:
|
||||
sn = d.get_info(rs.camera_info.serial_number)
|
||||
if want_sn and sn == want_sn:
|
||||
pick = d
|
||||
break
|
||||
if pick is None:
|
||||
for d in ds:
|
||||
name = (d.get_info(rs.camera_info.name) or "").lower()
|
||||
if "405" in name:
|
||||
pick = d
|
||||
break
|
||||
pick = pick or ds[0]
|
||||
usb = pick.get_info(rs.camera_info.usb_type_descriptor)
|
||||
sn = pick.get_info(rs.camera_info.serial_number)
|
||||
name = pick.get_info(rs.camera_info.name)
|
||||
print(f"{usb}|{sn}|{name}")
|
||||
PY
|
||||
)"
|
||||
USB_TYPE="${USB_INFO%%|*}"
|
||||
echo "=== RealSense USB=${USB_TYPE:-unknown} ${USB_INFO} ==="
|
||||
echo "=== site: ${SITE_CONFIG:-?} weights=${SITE_YOLO_WEIGHTS:-?} ==="
|
||||
|
||||
if [[ "${USB_TYPE}" == 2* ]]; then
|
||||
CAM_W=640; CAM_H=480; CAM_FPS=15
|
||||
echo "=== camera 640x480@15 (USB2: depth+IR still ON) ==="
|
||||
echo "=== WARN: D405 is on USB2. Plug it into a USB3 (blue) port for 848x480@30 ==="
|
||||
elif [[ "${SITE_CAM_USE_SITE_RES:-1}" == "1" ]]; then
|
||||
# site.yaml camera.width/height/fps 优先；--30fps 只抬帧率，不再写死 848x480
|
||||
CAM_W="${SITE_CAM_WIDTH:-848}"
|
||||
CAM_H="${SITE_CAM_HEIGHT:-480}"
|
||||
CAM_FPS="${SITE_CAM_FPS:-30}"
|
||||
if [[ "${HIGH_FPS}" -eq 1 && "${CAM_FPS}" -lt 30 ]]; then
|
||||
CAM_FPS=30
|
||||
fi
|
||||
echo "=== camera ${CAM_W}x${CAM_H}@${CAM_FPS} serial=${SITE_CAM_SERIAL:-} arm=${SITE_ROBOT_SN:-} ==="
|
||||
elif [[ "${HIGH_FPS}" -eq 1 ]]; then
|
||||
CAM_W=848; CAM_H=480; CAM_FPS=30
|
||||
echo "=== camera 848x480@30 (--30fps, site use_site_resolution=false) ==="
|
||||
else
|
||||
CAM_W=1280; CAM_H=720; CAM_FPS=15
|
||||
echo "=== camera 1280x720@15 ==="
|
||||
fi
|
||||
|
||||
CAM_ARGS=(
|
||||
--ros-args
|
||||
-p camera_name:="${CAM_NAME}"
|
||||
-p width:="${CAM_W}"
|
||||
-p height:="${CAM_H}"
|
||||
-p fps:="${CAM_FPS}"
|
||||
)
|
||||
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
|
||||
CAM_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
|
||||
fi
|
||||
CAM_ARGS+=(
|
||||
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
|
||||
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-4000}"
|
||||
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
|
||||
-p laser_power:="${SITE_CAM_LASER_POWER:-0}"
|
||||
)
|
||||
if [[ -n "${SITE_CAM_EXPOSURE_US:-}" && "${SITE_CAM_EXPOSURE_US}" != "0" && "${SITE_CAM_EXPOSURE_US}" != "0.0" ]]; then
|
||||
CAM_ARGS+=(-p exposure_us:="${SITE_CAM_EXPOSURE_US}")
|
||||
fi
|
||||
if [[ -n "${SITE_CAM_GAIN:-}" && "${SITE_CAM_GAIN}" != "0" && "${SITE_CAM_GAIN}" != "0.0" ]]; then
|
||||
CAM_ARGS+=(-p gain:="${SITE_CAM_GAIN}")
|
||||
fi
|
||||
|
||||
if [[ "${SITE_CAM_ENABLE_DEPTH:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p enable_depth:=true)
|
||||
echo "=== depth ON ==="
|
||||
else
|
||||
CAM_ARGS+=(-p enable_depth:=false)
|
||||
echo "=== depth OFF (site.yaml) ==="
|
||||
fi
|
||||
|
||||
DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
|
||||
if [[ "${PREVIEW}" -ge 0 ]]; then
|
||||
DO_PREVIEW="${PREVIEW}"
|
||||
fi
|
||||
# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。
|
||||
# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p enable_infra:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p enable_infra:=false)
|
||||
fi
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p preview_stereo:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p preview_stereo:=false)
|
||||
fi
|
||||
|
||||
if [[ "${DO_PREVIEW}" == "1" ]]; then
|
||||
echo "=== SDK preview ON (separate viewer process, hold ${SITE_CAM_OVERLAY_HOLD:-0.1}s) ==="
|
||||
echo "=== close window = hide preview only; camera keeps publishing ==="
|
||||
fi
|
||||
|
||||
echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ==="
|
||||
echo "=== bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ==="
|
||||
|
||||
if [[ "${DO_PREVIEW}" == "1" ]]; then
|
||||
# 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
|
||||
# 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
|
||||
# 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。
|
||||
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
|
||||
CAM_PID=$!
|
||||
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
|
||||
ros2 run arm_application realsense_view --ros-args \
|
||||
-p camera_name:="${CAM_NAME}" \
|
||||
-p overlay_hold_sec:="${SITE_CAM_OVERLAY_HOLD:-0.1}" \
|
||||
-p hud_label:="${SITE_ROBOT_LABEL:-} ${SITE_ROBOT_SN:-} ${SITE_CAM_SERIAL:-}"
|
||||
else
|
||||
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
|
||||
fi
|
||||
Executable
+60
@@ -0,0 +1,60 @@
|
||||
#!/usr/bin/env bash
|
||||
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。
|
||||
# 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。
|
||||
#
|
||||
# bash ~/ros2/scripts/run_camera_d435.sh --preview
|
||||
#
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
PREVIEW=1
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--preview) PREVIEW=1; shift ;;
|
||||
--no-preview) PREVIEW=0; shift ;;
|
||||
-h|--help) sed -n '2,8p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
SN="${SITE_D435_SERIAL:-}"
|
||||
if [[ -z "${SN}" ]]; then
|
||||
echo "ERROR: cameras.d435.serial empty in site.yaml" >&2
|
||||
exit 2
|
||||
fi
|
||||
echo "=== D435 overhead serial=${SN} ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
|
||||
# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉，
|
||||
# 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。
|
||||
pkill -f "camera_name:=d435( |$)" 2>/dev/null || true
|
||||
CAM_ARGS=(
|
||||
--ros-args
|
||||
-p camera_name:=d435
|
||||
-p camera_serial:="${SN}"
|
||||
-p width:="${SITE_D435_WIDTH:-1280}"
|
||||
-p height:="${SITE_D435_HEIGHT:-720}"
|
||||
-p fps:="${SITE_D435_FPS:-30}"
|
||||
-p enable_depth:=true
|
||||
-p enable_infra:=false
|
||||
-p preview_stereo:=false
|
||||
-p auto_exposure:=true
|
||||
-p ae_limit_us:=8000
|
||||
-p emitter_enabled:=true
|
||||
)
|
||||
if [[ "${PREVIEW}" == "1" ]]; then
|
||||
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
|
||||
CAM_PID=$!
|
||||
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
|
||||
# 绿框来自 workpiece/detection_overhead（D435 像素）。不要订 workpiece/detection。
|
||||
ros2 run arm_application realsense_view --ros-args \
|
||||
-p camera_name:=d435 \
|
||||
-p overlay_detection:=true \
|
||||
-p detection_topic:=workpiece/detection_overhead \
|
||||
-p overlay_hold_sec:=1.5 \
|
||||
-p hud_label:="D435 顶视"
|
||||
else
|
||||
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
|
||||
fi
|
||||
Executable
+136
@@ -0,0 +1,136 @@
|
||||
#!/usr/bin/env bash
|
||||
# 真机 Rizon4 启动入口 —— 设置 RDK 运行时库，然后以 backend:=real 启动整栈
|
||||
# 注意：这里不加 `set -u` —— conda/RoboStack 的钩子会引用未绑定的变量（CONDA_BUILD、COLCON_TRACE）
|
||||
#
|
||||
# bash ~/ros2/scripts/run_real.sh
|
||||
# bash ~/ros2/scripts/run_real.sh --arm 91
|
||||
# bash ~/ros2/scripts/run_real.sh --sn Rizon4-063991 # 本次启动换臂（不改 yaml）
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
FLEXIV_PREFIX="${HOME}/flexiv_install"
|
||||
LIST_ONLY=false
|
||||
NO_STOP=false
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
|
||||
--no-stop) NO_STOP=true; shift ;;
|
||||
--list|-l) LIST_ONLY=true; shift ;;
|
||||
-h|--help)
|
||||
sed -n '2,8p' "$0"
|
||||
echo " 机械臂模型在工位程序里看，这里默认不开 RViz。"
|
||||
echo " 其余参数传给 ros2 launch。要临时打开 RViz：use_rviz:=true"
|
||||
exit 0
|
||||
;;
|
||||
*) break ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
dev_source_conda
|
||||
conda activate ros_flexiv
|
||||
if [[ ! -f "${ROOT}/install/setup.bash" ]]; then
|
||||
echo "ERROR: ${ROOT}/install/setup.bash 不存在。先在仓库根目录 colcon build，不要在中文路径下编。" >&2
|
||||
exit 1
|
||||
fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/install/setup.bash"
|
||||
# 本机同时有 /opt/ros/humble。install 会把它排到 conda 前面，
|
||||
# 3.10 的 ros2 会去加载 ros_flexiv 里 3.11 的 rclpy。把 conda 的 ros2 放回最前。
|
||||
if [[ -n "${CONDA_PREFIX:-}" && -x "${CONDA_PREFIX}/bin/ros2" ]]; then
|
||||
export PATH="${CONDA_PREFIX}/bin:${PATH}"
|
||||
fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
# 真机与仿真的 DDS 域隔离：这个入口**不经过** env.sh，所以这里也得纠正一次。
|
||||
# shellcheck source=lib/ros_domain.sh
|
||||
source "${ROOT}/scripts/lib/ros_domain.sh"
|
||||
ros_domain_force_real
|
||||
|
||||
# `flexiv_hardware` 必须从 flexiv_install 加载 spdlog/RBDyn（不能用 conda 的 fmt v11）
|
||||
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
|
||||
|
||||
# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号（89/91 别名已经映射好了）。
|
||||
ROBOT_SN="${SITE_ROBOT_SN:?run_real.sh 需要已配置的机械臂 SN —— 见 scripts/lib/load_site_config.sh 的报错}"
|
||||
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
||||
NET_IFACE="${SITE_NET_IFACE:-}"
|
||||
RDK_PYTHON="$(dev_rdk_python)"
|
||||
|
||||
if [[ "${LIST_ONLY}" == true ]]; then
|
||||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print
|
||||
exit 0
|
||||
fi
|
||||
|
||||
echo "=== site: ${SITE_CONFIG:-n/a} SN=${ROBOT_SN} ${SITE_ROBOT_LABEL:-} arm=${SITE_ROBOT_ARM:-} ==="
|
||||
echo "=== catalog: ${SITE_ROBOT_CATALOG:-} ==="
|
||||
echo "=== handeye: ${SITE_HANDEYE_FILE:-} ==="
|
||||
echo "=== camera serial: ${SITE_CAM_SERIAL:-} gripper: ${SITE_GRIP_MODEL:-} ${SITE_GRIP_PORT:-} ==="
|
||||
echo "=== network iface=${NET_IFACE:-auto} robot=${ROBOT_IP} pc=${SITE_PC_IP:-} ==="
|
||||
if [[ "${NO_STOP}" == true ]]; then
|
||||
echo "=== --no-stop：不扫 install/（感知和伺服还在，扫了会把自己杀掉）==="
|
||||
else
|
||||
echo "=== stop leftover real stack (RDK exclusive) ==="
|
||||
# 以前这里是两行 pkill（ros2_control_node + stack.launch），漏掉了
|
||||
# arm_control 那一族**独立存活**的孤儿（adapter / named_pose_server /
|
||||
# arm_status_node / joint_state_publisher）—— 它们会冒名顶替新一轮的栈，
|
||||
# 见 lib/real_procs.sh 的文件头。扫描规则与 stop_real.sh 共用一份。
|
||||
# shellcheck source=lib/real_procs.sh
|
||||
source "${ROOT}/scripts/lib/real_procs.sh"
|
||||
mapfile -t _stale < <(real_proc_targets)
|
||||
if [ "${#_stale[@]}" != "0" ]; then
|
||||
echo "清掉上一轮残留（${#_stale[@]} 个）："
|
||||
for _pid in "${_stale[@]}"; do
|
||||
printf ' %-7s %s\n' "$_pid" "$(ps -o args= -p "$_pid" 2>/dev/null | cut -c1-120)"
|
||||
kill -TERM "$_pid" 2>/dev/null || true
|
||||
done
|
||||
sleep 1
|
||||
for _pid in "${_stale[@]}"; do
|
||||
kill -0 "$_pid" 2>/dev/null && kill -KILL "$_pid" 2>/dev/null || true
|
||||
done
|
||||
unset _pid
|
||||
fi
|
||||
unset _stale
|
||||
fi
|
||||
sleep 1
|
||||
# 清掉把 91 的 .100 误绑到 89 MAC 上的陈旧 ARP
|
||||
if [[ -n "${NET_IFACE}" ]]; then
|
||||
ip neigh del "${ROBOT_IP}" dev "${NET_IFACE}" 2>/dev/null || true
|
||||
fi
|
||||
if ping -c 2 -W 1 "${ROBOT_IP}" >/dev/null 2>&1; then
|
||||
echo "robot ${ROBOT_IP} ping OK"
|
||||
else
|
||||
echo "WARN: cannot ping ${ROBOT_IP} — 这一台不在网里，RDK 大概率会失败。"
|
||||
echo " 本机: $(ip -4 -o addr show dev "${NET_IFACE:-enp4s0}" 2>/dev/null | awk '{print $4}' | tr '\n' ' ')"
|
||||
echo " ARP ${ROBOT_IP}: $(ip neigh show "${ROBOT_IP}" 2>/dev/null | head -1 || echo none)"
|
||||
MAC91="$(ip neigh show "${ROBOT_IP}" 2>/dev/null | awk '{print $5}' | head -1)"
|
||||
MAC89="$(ip neigh show 192.168.2.101 2>/dev/null | awk '{print $5}' | head -1)"
|
||||
if [[ -n "${MAC91}" && -n "${MAC89}" && "${MAC91}" == "${MAC89}" ]]; then
|
||||
echo " ARP MAC 与 89(192.168.2.101) 相同 → .100 是误绑，91 并没有回包。"
|
||||
fi
|
||||
echo " 91 请接控制柜 User Port 1（出厂静态 192.168.2.100）到同一交换机，示教器 AUTO+Remote Ethernet。"
|
||||
echo " 89=192.168.2.101 91=192.168.2.100"
|
||||
fi
|
||||
|
||||
echo "=== RDK preflight + tool (${ROBOT_SN}) ==="
|
||||
TOOL_NAME="${SITE_ROBOT_TOOL_NAME:-}"
|
||||
RDK_TOOL_ARGS=(--sn "${ROBOT_SN}")
|
||||
if [[ -n "${NET_IFACE}" ]]; then
|
||||
RDK_TOOL_ARGS+=(--iface "${NET_IFACE}")
|
||||
fi
|
||||
if [[ -n "${TOOL_NAME}" ]]; then
|
||||
RDK_TOOL_ARGS+=(--name "${TOOL_NAME}")
|
||||
echo "=== Studio tool → ${TOOL_NAME} ==="
|
||||
fi
|
||||
if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}"; then
|
||||
echo ""
|
||||
echo "ERROR: RDK cannot connect / switch tool on ${ROBOT_SN}."
|
||||
echo " 先确认 ping ${ROBOT_IP} 通（91=控制柜 User Port 1 → 192.168.2.100）。"
|
||||
echo " Stop any other RDK client (this run_real.sh is exclusive)."
|
||||
echo " On teach pendant: Settings → Remote → enable Ethernet/RDK remote mode."
|
||||
echo " Test: bash ${ROOT}/scripts/test_rdk_python.sh ${ROBOT_SN}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" use_rviz:=false "$@"
|
||||
Reference in New Issue
Block a user