orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: 框架科学优化前的完整基线

Browse Source 
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态，
使后续任何删除/重构都可回退。

本基线包含：
- 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等）
- 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接）
- 设备契约层 src/arm_interfaces（新增，46 项单测通过）
- devices/ 设备选型与品牌清单（新增）
- scripts/gen_site_example.py 与由它生成的 config/site.example.yaml

按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
2026-09-23 10:53:06 +08:00
commit 6edb8d39ab
635 changed files with 1165230 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/README.md
+16
View File
@@ -0,0 +1,16 @@
# scripts — 唯一操作入口
实现细节在 [docs/WORKSPACE.md](../docs/WORKSPACE.md) 和 [docs/](../docs/)。
**每天：** `run_real.sh` · `run_camera_d405.sh` · `run_visual_servo_rt.sh` · `go_home.sh`
**手眼：** `run_handeye.sh`（`teach` / `merge` / `calibrate` / `show`）
**夹爪：** `run_gripper.sh --arm 89|91` · `run_dual_gripper.sh`
**入孔：** `run_hole_detect.sh --arm … [--preview]`
**触觉：** `run_tactile.sh [--cli]`
**权重：** `export_yolo_trt.sh`
**配置：** `show_site_config.sh` · `lib/export_site_env.py`
**环境：** `env.sh` · `build_all.sh`
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh`
`lib/parse_arm_args.sh` 解析 `--arm 89|91`。
`lib/load_site_config.sh` 把 `config/site.yaml` 变成 `SITE_*`。
scripts/build.sh
Executable
+32
View File
@@ -0,0 +1,32 @@
#!/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"
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_control arm_moveit_config arm_bringup arm_application)
colcon build --symlink-install --packages-select "${PACKAGES[@]}" "$@"
echo "构建完成。请执行: source ${ROOT}/install/setup.bash"
scripts/build_all.sh
Executable
+30
View File
@@ -0,0 +1,30 @@
#!/usr/bin/env bash
# Build entire workspace: arm stack + flexiv real drivers
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"
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"
scripts/build_flexiv_rdk.sh
Executable
+34
View File
@@ -0,0 +1,34 @@
#!/usr/bin/env bash
# Build Flexiv RDK v1.x into ~/flexiv_install (required for real robot / flexiv_hardware).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
RDK="${ROOT}/src/flexiv_rdk"
PREFIX="${FLEXIV_INSTALL_PREFIX:-$HOME/flexiv_install}"
if [[ ! -d "$RDK" ]]; then
echo "Missing $RDK — run scripts/fetch_flexiv.sh first"
exit 1
fi
echo "Building RDK thirdparty deps into $PREFIX ..."
# Prefer system /opt/ros if present; else conda env already has compilers
if [[ -f /opt/ros/humble/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
fi
cd "$RDK"
if [[ -f thirdparty/build_and_install_dependencies_not_in_ros2.sh ]]; then
bash thirdparty/build_and_install_dependencies_not_in_ros2.sh "$PREFIX"
elif [[ -f thirdparty/build_and_install_dependencies.sh ]]; then
bash thirdparty/build_and_install_dependencies.sh "$PREFIX"
else
echo "No thirdparty build script found; see RDK README"
ls thirdparty || true
fi
rm -rf build && mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX="$PREFIX" -DCMAKE_BUILD_TYPE=Release
cmake --build . --target install -j"$(nproc)"
echo "RDK installed to $PREFIX"
echo "Then: cd $ROOT && colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=$PREFIX"
scripts/build_real.sh
Executable
+36
View File
@@ -0,0 +1,36 @@
#!/usr/bin/env bash
# Build Flexiv hardware packages for real robot (after RDK installed to ~/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"
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
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"
scripts/check_robot_network.sh
Executable
+90
View File
@@ -0,0 +1,90 @@
#!/usr/bin/env bash
# Check PC ↔ Flexiv robot network (addresses from site.yaml)
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
ROBOT_SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
PC_IP="${PC_IP:-${SITE_PC_IP:-192.168.100.10}}"
pick_iface() {
if [[ -n "${IFACE:-}" ]]; then
echo "$IFACE"
return
fi
if [[ -n "${SITE_NET_IFACE:-}" && -d "/sys/class/net/${SITE_NET_IFACE}" ]]; then
echo "${SITE_NET_IFACE}"
return
fi
local d
for d in enp4s0 enp5s0 eth0; do
[[ -d /sys/class/net/$d ]] && echo "$d" && return
done
for d in $(ls /sys/class/net); do
[[ "$d" == lo ]] && continue
[[ "$d" == en* || "$d" == eth* ]] || continue
local state
state=$(cat /sys/class/net/"$d"/operstate 2>/dev/null || echo down)
[[ "$state" == up ]] && echo "$d" && return
done
echo enp4s0
}
IFACE="$(pick_iface)"
echo "=== 本机网卡 ==="
ip -4 -br addr
echo
echo "=== 目标机器人（site.yaml） ==="
echo " SN $ROBOT_SN"
echo " 标签 ${SITE_ROBOT_LABEL:-}"
echo " 目录 ${SITE_ROBOT_CATALOG:-}"
echo " IP $ROBOT_IP"
echo " 本机 $PC_IP"
echo " 网卡 $IFACE"
if ping -c 1 -W 1 "$ROBOT_IP" >/dev/null 2>&1; then
echo " ping UP — 链路 OK"
else
echo " ping DOWN — PC 与机器人必须同网段（本机 ${PC_IP} ↔ 机器人 ${ROBOT_IP}）"
echo
echo " 一键修复（交换机 / 有线）："
echo " bash ~/ros2/scripts/fix_robot_lan.sh"
echo " 独立交换机（不要办公网 DHCP）可用："
echo " bash ~/ros2/scripts/fix_robot_lan.sh --only"
fi
echo
echo "=== 常见地址探测 ==="
CANDIDATES=(
"$ROBOT_IP"
192.168.2.100 192.168.2.10 192.168.101.1 192.168.101.10
192.168.100.1 192.168.100.10
192.168.100.1 192.168.100.2 192.168.100.10
192.168.1.1
)
seen=""
for ip in "${CANDIDATES[@]}"; do
[[ " ${seen} " == *" ${ip} "* ]] && continue
seen+=" ${ip}"
if ping -c 1 -W 1 "$ip" >/dev/null 2>&1; then
echo " UP $ip"
else
echo " down $ip"
fi
done
echo
echo "=== 当前邻居 (ARP) ==="
ip neigh | grep -v FAILED || true
echo
echo "说明："
echo "1) 网线接机器人控制柜网口 ↔ 本机 $IFACE。"
echo "2) 示教器开启 RDK / 远程控制后再连。"
echo "3) RDK 连接用 SN（不是 IP）：flexivrdk.Robot('$ROBOT_SN')"
echo "4) 切换机械臂：改 ~/ros2/config/site.yaml 的 robot.sn"
echo "5) 手册: https://flexiv.cn/software/rdk/manual/index.html"
scripts/convert_flexiv_traj.py
Executable
+117
View File
@@ -0,0 +1,117 @@
#!/usr/bin/env python3
"""Convert Flexiv Elements .traj (text) -> arm_application YAML waypoints."""
from __future__ import annotations
import argparse
import math
import re
import sys
from pathlib import Path
from typing import Any
def parse_flexiv_traj(text: str) -> list[dict[str, Any]]:
"""Parse Elements Studio scatter-point .traj file."""
blocks = re.split(r'pattern_params\s*\{', text)[1:]
waypoints: list[dict[str, Any]] = []
for block in blocks:
name_m = re.search(r'pattern_name:\s*"([^"]+)"', block)
vel_m = re.search(r'velocity:\s*"([^"]+)"', block)
joints: dict[str, float] = {}
for j in range(1, 8):
m = re.search(rf'joint{j}:\s*"([^"]+)"', block)
if m:
joints[f'joint{j}'] = math.radians(float(m.group(1)))
if len(joints) != 7:
continue
positions = [joints[f'joint{i}'] for i in range(1, 8)]
waypoints.append({
'name': name_m.group(1) if name_m else f'WPT{len(waypoints)}',
'positions': positions,
'velocity': float(vel_m.group(1)) if vel_m else 0.2,
})
return waypoints
def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None:
"""Cumulative time_from_start from joint-space segment length."""
t = 0.0
prev = None
for wp in waypoints:
pos = wp['positions']
if prev is None:
t += min_seg
else:
delta = max(abs(a - b) for a, b in zip(pos, prev))
# ~4 s per radian max joint move, floored at min_seg
t += max(min_seg, delta * 4.0)
wp['time'] = round(t, 2)
prev = pos
def to_yaml(
waypoints: list[dict[str, Any]],
*,
joint_prefix: str = 'Rizon4-063989_',
velocity_scale: float | None = None,
) -> str:
if velocity_scale is None:
velocity_scale = waypoints[0].get('velocity', 0.2) if waypoints else 0.2
times = [wp['time'] for wp in waypoints]
flat: list[float] = []
for wp in waypoints:
flat.extend(wp['positions'])
pos_line = ', '.join(f'{v:.6f}' for v in flat)
time_line = ', '.join(str(t) for t in times)
names = ', '.join(f' # {wp.get("name", "")}' if i == 0 else '' for i, wp in enumerate(waypoints))
_ = names # names kept in comments below
lines = [
'# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)',
'# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)',
'# Edit velocity_scale / loop_count as needed (no rebuild).',
'run_joint_trajectory:',
' ros__parameters:',
f' joint_prefix: "{joint_prefix}"',
' joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]',
f' velocity_scale: {velocity_scale}',
' loop_count: 1 # 1=once, 0=forever, N=repeat N',
' pause_between_loops: 0.5',
f' waypoint_times: [{time_line}]',
' waypoint_positions: [',
]
for i, wp in enumerate(waypoints):
pos = ', '.join(f'{v:.6f}' for v in wp['positions'])
lines.append(f' {pos}, # {wp.get("name", f"WPT{i}")}')
lines.append(' ]')
return '\n'.join(lines) + '\n'
def main() -> int:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument('input', type=Path, help='Flexiv Elements .traj file')
ap.add_argument('-o', '--output', type=Path, help='Output YAML path')
ap.add_argument('--prefix', default='Rizon4-063989_', help='Joint name prefix (site SN + _)')
ap.add_argument('--min-seg', type=float, default=3.0, help='Min seconds per segment')
args = ap.parse_args()
text = args.input.read_text(encoding='utf-8')
wps = parse_flexiv_traj(text)
if not wps:
print('ERROR: no waypoints parsed', file=sys.stderr)
return 1
assign_times(wps, min_seg=args.min_seg)
yaml_text = to_yaml(wps, joint_prefix=args.prefix)
out = args.output
if out:
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(yaml_text, encoding='utf-8')
print(f'Wrote {len(wps)} waypoints -> {out}')
else:
print(yaml_text)
return 0
if __name__ == '__main__':
raise SystemExit(main())
scripts/convert_traj.sh
Executable
+69
View File
@@ -0,0 +1,69 @@
#!/usr/bin/env bash
# Convert Flexiv Elements .traj → arm_application YAML (standard entry for all trajectories).
# Usage:
# ./scripts/convert_traj.sh ~/文档/my_task.traj
# ./scripts/convert_traj.sh ~/文档/my_task.traj -o /path/to/out.yaml
# ./scripts/convert_traj.sh ~/文档/my_task.traj --min-seg 2.5 --no-copy
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PY="${ROOT}/scripts/convert_flexiv_traj.py"
if [[ $# -lt 1 ]]; then
echo "Usage: $0 INPUT.traj [-o OUT.yaml] [--min-seg SECONDS] [--no-copy]"
echo "Example: $0 ~/文档/my_task.traj"
exit 1
fi
INPUT="$(realpath "$1")"
shift
BASENAME="$(basename "${INPUT}" .traj)"
OUT_DEFAULT="${ROOT}/src/arm_application/config/${BASENAME}_trajectory.yaml"
TRAJ_COPY="${ROOT}/src/arm_application/trajectories/${BASENAME}.traj"
OUT="${OUT_DEFAULT}"
MIN_SEG="3.0"
DO_COPY=1
EXTRA=()
while [[ $# -gt 0 ]]; do
case "$1" in
-o|--output)
OUT="$(realpath -m "$2")"
shift 2
;;
--min-seg)
MIN_SEG="$2"
shift 2
;;
--no-copy)
DO_COPY=0
shift
;;
*)
EXTRA+=("$1")
shift
;;
esac
done
if [[ ! -f "${INPUT}" ]]; then
echo "ERROR: input not found: ${INPUT}" >&2
exit 1
fi
python3 "${PY}" "${INPUT}" -o "${OUT}" --min-seg "${MIN_SEG}" "${EXTRA[@]}"
if [[ "${DO_COPY}" -eq 1 ]]; then
mkdir -p "$(dirname "${TRAJ_COPY}")"
cp -f "${INPUT}" "${TRAJ_COPY}"
echo "Copied source traj → ${TRAJ_COPY}"
fi
echo ""
echo "Next (stack must be running):"
echo " conda activate ros_flexiv"
echo " source ${ROOT}/install/setup.bash"
echo " export LD_LIBRARY_PATH=~/flexiv_install/lib:\$LD_LIBRARY_PATH"
echo " ros2 run arm_application run_joint_trajectory --ros-args \\"
echo " --params-file ${OUT}"
scripts/env.sh
Executable
+36
View File
@@ -0,0 +1,36 @@
#!/usr/bin/env bash
# Source this workspace environment (conda + ROS + flexiv libs).
# Usage: 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
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
conda activate ros_flexiv 2>/dev/null || true
fi
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
# Pip TensorRT (tensorrt-cu12) — needed to load YOLO .engine
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/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
export YOLO_AUTOINSTALL=false
export ROS2_WS="${ROOT}"
# Master site config → SITE_* (YOLO weights, camera, robot SN, …)
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
scripts/export_yolo_trt.sh
Executable
+90
View File
@@ -0,0 +1,90 @@
#!/usr/bin/env bash
# Export YOLO .pt/.onnx → TensorRT .engine (FP16).
#
# bash ~/ros2/scripts/export_yolo_trt.sh
# bash ~/ros2/scripts/export_yolo_trt.sh /home/cc/ros2/models/yolo/detect_best.onnx 640
#
# Needs: tensorrt-cu12 + onnx + onnxslim in ros_flexiv (do NOT pip install
# the meta package `tensorrt`, it pulls CUDA 13 wheels).
# Then set yolo.backend: tensorrt in site.yaml and restart visual servo.
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
PT="${1:-${SITE_YOLO_WEIGHTS:-${ROOT}/models/yolo/detect_best.onnx}}"
IMGSZ="${2:-${SITE_YOLO_IMGSZ:-640}}"
PY="${HOME}/anaconda3/envs/ros_flexiv/bin/python"
if [[ ! -f "${PT}" ]]; then
echo "ERROR: weights not found: ${PT}" >&2
exit 1
fi
# Pip TensorRT shared libs (needed at export + runtime).
TRT_LIBS="$("${PY}" - <<'PY'
import pathlib, tensorrt
p = pathlib.Path(tensorrt.__file__).resolve().parent
cands = [p, p.parent / "tensorrt_libs", p.parent / "nvidia" / "tensorrt" / "lib"]
print(":".join(str(x) for x in cands if x.exists()))
PY
)"
export LD_LIBRARY_PATH="${TRT_LIBS}${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}"
export YOLO_AUTOINSTALL=false
echo "Export TensorRT engine from ${PT} imgsz=${IMGSZ}"
"${PY}" - <<PY
from pathlib import Path
import tensorrt as trt
src = Path("${PT}").expanduser()
print("tensorrt", trt.__version__)
print("source", src)
engine_path = src.with_suffix(".engine")
if src.suffix.lower() == ".onnx":
logger = trt.Logger(trt.Logger.INFO)
flag = 1 << int(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH)
builder = trt.Builder(logger)
network = builder.create_network(flag)
parser = trt.OnnxParser(network, logger)
raw = src.read_bytes()
if not parser.parse(raw):
for i in range(parser.num_errors):
print("ONNX parse error:", parser.get_error(i))
raise SystemExit("ONNX parse failed")
config = builder.create_builder_config()
config.set_memory_pool_limit(trt.MemoryPoolType.WORKSPACE, 4 << 30)
if builder.platform_has_fast_fp16:
config.set_flag(trt.BuilderFlag.FP16)
print("FP16 enabled")
serialized = builder.build_serialized_network(network, config)
if serialized is None:
raise SystemExit("TensorRT build failed")
engine_path.write_bytes(bytes(serialized))
print("wrote", engine_path)
else:
from ultralytics import YOLO
m = YOLO(str(src))
try:
out = m.export(
format="engine",
imgsz=int("${IMGSZ}"),
device=0,
quantize=16,
simplify=True,
workspace=4,
)
except TypeError:
out = m.export(
format="engine",
imgsz=int("${IMGSZ}"),
device=0,
half=True,
simplify=True,
workspace=4,
)
print("wrote", out)
PY
echo "Done. Restart: bash ~/ros2/scripts/run_visual_servo_rt.sh --control"
scripts/fetch_flexiv.sh
Executable
+116
View File
@@ -0,0 +1,116 @@
#!/usr/bin/env bash
# Clone official Flexiv ROS 2 stack for Rizon (RDK v1.x → humble-v1 branch).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
BRANCH="${FLEXIV_ROS2_BRANCH:-humble-v1}"
mkdir -p "${SRC}"
cd "${SRC}"
# Always ensure official URDF package is present (meshes + xacro)
if [[ ! -d flexiv_description ]]; then
echo "Fetching flexiv_description (${BRANCH})..."
if ! git clone --depth 1 -b "${BRANCH}" https://github.com/flexivrobotics/flexiv_description.git; then
echo "git clone slow/failed — downloading zip..."
python3 - <<PY
import urllib.request, socket, zipfile, io, shutil, os
url = "https://codeload.github.com/flexivrobotics/flexiv_description/zip/refs/heads/${BRANCH}"
old = socket.getaddrinfo
socket.getaddrinfo = lambda h, p, f=0, t=0, pr=0, fl=0: [ai for ai in old(h, p, socket.AF_INET, t, pr, fl)]
data = urllib.request.urlopen(urllib.request.Request(url, headers={"User-Agent": "Mozilla/5.0"}), timeout=300).read()
dest = "${SRC}/flexiv_description"
with zipfile.ZipFile(io.BytesIO(data)) as z:
z.extractall("/tmp")
shutil.rmtree(dest, ignore_errors=True)
shutil.move("/tmp/flexiv_description-${BRANCH}", dest)
print("flexiv_description ready at", dest)
PY
fi
else
echo "flexiv_description already present"
fi
if [[ ! -d flexiv_ros2 ]]; then
echo "Cloning flexiv_ros2 (${BRANCH})..."
git clone https://github.com/flexivrobotics/flexiv_ros2.git -b "${BRANCH}"
else
echo "flexiv_ros2 already present — pull if needed: git -C flexiv_ros2 pull"
fi
REPOS_FILE=""
for candidate in flexiv_ros2/flexiv.humble.repos flexiv_ros2/flexiv.repos; do
if [[ -f "${candidate}" ]]; then
REPOS_FILE="${candidate}"
break
fi
done
if [[ -z "${REPOS_FILE}" ]]; then
echo "ERROR: no .repos file found under flexiv_ros2"
exit 1
fi
echo "Importing dependencies from ${REPOS_FILE}..."
if ! command -v vcs >/dev/null 2>&1; then
echo "Please install: sudo apt install python3-vcstool"
exit 1
fi
# Prefer local Flexiv_Rizon4 RDK (avoids GitHub clone when present)
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
echo "Using local RDK: ${LOCAL_FLEXIV}/flexiv_rdk"
bash "${ROOT}/scripts/use_local_flexiv.sh"
elif [[ -d flexiv_rdk ]]; then
touch flexiv_rdk/COLCON_IGNORE
else
echo "Importing flexiv_rdk from ${REPOS_FILE}..."
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing
if [[ -d flexiv_rdk ]]; then
touch flexiv_rdk/COLCON_IGNORE
fi
fi
# Always import remaining deps from repos (description already handled; skip-existing)
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing || true
cat <<EOF
Done. Next steps (Ubuntu 22.04 / Humble + Rizon):
Local RDK wired via: scripts/use_local_flexiv.sh
src/flexiv_rdk -> ${LOCAL_FLEXIV}/flexiv_rdk (your copy, often RDK 2.x)
src/flexiv_rdk_v1 -> RDK 1.9.1 for flexiv_ros2 humble-v1 (if present)
1) Install ROS deps:
sudo apt install -y python3-colcon-common-extensions python3-vcstool \\
ros-humble-ros2-control ros-humble-ros2-controllers \\
ros-humble-moveit ros-humble-xacro ros-humble-joint-state-publisher-gui
2) Build & install RDK **v1.9.1** into ~/flexiv_install (for flexiv_hardware):
RDK_SRC=${SRC}/flexiv_rdk_v1
# fallback if only one tree: RDK_SRC=${SRC}/flexiv_rdk
cd \${RDK_SRC}/thirdparty
source /opt/ros/humble/setup.bash # or: conda activate ros_flexiv
bash build_and_install_dependencies_not_in_ros2.sh ~/flexiv_install
cd \${RDK_SRC} && rm -rf build && mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install -DCMAKE_PREFIX_PATH=~/flexiv_install
cmake --build . --target install --config Release
3) Build this workspace:
cd ${ROOT}
source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/flexiv_install
source install/setup.bash
4) Real robot (site SN, no gripper yet):
ros2 launch arm_bringup real.launch.py
# or explicitly:
ros2 launch arm_bringup real.launch.py robot_sn:=Rizon4-063989 load_gripper:=false
Official fake hardware:
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=Rizon4-063989 use_fake_hardware:=true load_gripper:=false
EOF
scripts/fetch_isaac_flexiv.sh
Executable
+46
View File
@@ -0,0 +1,46 @@
#!/usr/bin/env bash
# Clone Flexiv Isaac Sim integration repos into thirdparty/
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
TP="${ROOT}/thirdparty"
mkdir -p "${TP}"
cd "${TP}"
clone_or_update() {
local url="$1"
local dir="$2"
if [[ -d "${dir}/.git" ]]; then
echo "Updating ${dir}..."
git -C "${dir}" pull --ff-only || true
else
echo "Cloning ${dir}..."
git clone --depth 1 "${url}" "${dir}"
fi
}
clone_or_update https://github.com/flexivrobotics/flexiv_sim_plugin.git flexiv_sim_plugin
clone_or_update https://github.com/flexivrobotics/isaac_sim_ws.git isaac_sim_ws
clone_or_update https://github.com/flexivrobotics/flexiv_description.git flexiv_description
# Optional: RDK sources next to sim (if not already under src/)
if [[ ! -d "${ROOT}/src/flexiv_rdk" ]]; then
clone_or_update https://github.com/flexivrobotics/flexiv_rdk.git flexiv_rdk
fi
cat <<EOF
Isaac / description repos are under: ${TP}
Next (Isaac path):
1) Install NVIDIA Isaac Sim
2) Follow flexiv_sim_plugin README (Elements Studio → External / Isaac)
3) cd ${TP}/isaac_sim_ws && bash install_ws.sh <isaac_sim_root>
4) Configure robot instance for Rizon4-063989 in app_config.yaml
5) Primitive docs: https://primitive.flexiv.cn/primitives/zh-CN/3.10/rizon4/index.html
6) RDK manual (CN): https://flexiv.cn/software/rdk/manual/index.html
ROS 2 path (separate):
${ROOT}/scripts/fetch_flexiv.sh
EOF
scripts/fix_robot_lan.sh
Executable
+146
View File
@@ -0,0 +1,146 @@
#!/usr/bin/env bash
# 工控机 User1（enp4s0）↔ 交换机 ↔ 两台 Flexiv
#
# 本机 192.168.2.10/24（User1）。两台臂：063989=192.168.2.101 063991=192.168.2.100
#
# bash ~/ros2/scripts/fix_robot_lan.sh
# bash ~/ros2/scripts/fix_robot_lan.sh --only # 不要办公网 DHCP（独立交换机）
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
PC_IP_PRIMARY="${SITE_PC_IP:-192.168.2.10}"
PC_ADDRS=("192.168.2.10/24" "192.168.100.10/24")
ROBOT_IPS=("${SITE_ROBOT_IP:-192.168.2.100}")
# Catalog extras (second arm)
if [[ -n "${SITE_ROBOT_CATALOG:-}" ]]; then
:
fi
MODE=secondary
while [[ $# -gt 0 ]]; do
case "$1" in
--only) MODE=only; shift ;;
--secondary) MODE=secondary; shift ;;
-h|--help)
sed -n '2,12p' "$0"
exit 0
;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
pick_iface() {
if [[ -n "${IFACE:-}" ]]; then
echo "$IFACE"
return
fi
if [[ -n "${SITE_NET_IFACE:-}" && -d "/sys/class/net/${SITE_NET_IFACE}" ]]; then
echo "${SITE_NET_IFACE}"
return
fi
for d in enp4s0 enp5s0 eth0; do
[[ -d /sys/class/net/$d ]] || continue
echo "$d"
return
done
return 1
}
IFACE="$(pick_iface)" || {
echo "ERROR: 找不到 User1 有线网卡" >&2
ip -br link
exit 1
}
ADDR_CSV="$(IFS=,; echo "${PC_ADDRS[*]}")"
echo "=== User1 / ${IFACE} 模式=${MODE} ==="
echo "本机副 IP: ${ADDR_CSV}"
echo "当前臂: SN=${SITE_ROBOT_SN:-?} IP=${SITE_ROBOT_IP:-?} catalog=${SITE_ROBOT_CATALOG:-}"
ip -4 -br addr
echo
if ! command -v nmcli >/dev/null; then
echo "ERROR: 需要 nmcli" >&2
exit 1
fi
CON="$(nmcli -t -f NAME,DEVICE connection show --active | awk -F: -v d="$IFACE" '$2==d{print $1; exit}')"
if [[ -z "$CON" ]]; then
CON="$(nmcli -t -f NAME,DEVICE connection show | awk -F: -v d="$IFACE" '$2==d{print $1; exit}')"
fi
if [[ -z "$CON" ]]; then
echo "ERROR: NetworkManager 没有绑定 $IFACE" >&2
nmcli device status
exit 1
fi
echo "NM 连接: $CON"
nmcli_mod() {
if nmcli connection modify "$CON" "$@"; then
return 0
fi
sudo nmcli connection modify "$CON" "$@"
}
nmcli_up() {
if nmcli connection up "$CON"; then
return 0
fi
sudo nmcli connection up "$CON"
}
if [[ "$MODE" == only ]]; then
nmcli_mod \
ipv4.method manual \
ipv4.addresses "${PC_IP_PRIMARY}/24" \
ipv4.gateway "" \
ipv4.dns "" \
ipv4.ignore-auto-dns yes \
ipv4.never-default yes
else
nmcli_mod \
ipv4.method auto \
ipv4.addresses "${ADDR_CSV}" \
ipv4.never-default no
fi
nmcli_up
echo
echo "=== 修复后 ==="
ip -4 -br addr show "$IFACE"
echo
ping_one() {
local ip="$1"
if ping -c 2 -W 1 "$ip" >/dev/null 2>&1; then
echo " UP $ip"
return 0
fi
echo " down $ip"
return 1
}
echo "=== ping 机器人网段 ==="
ok=0
for ip in 192.168.2.100 192.168.101.1 192.168.2.101 "${SITE_ROBOT_IP:-}"; do
[[ -z "$ip" ]] && continue
ping_one "$ip" && ok=1 || true
done
# unique
echo
if ping -c 1 -W 1 "${SITE_ROBOT_IP:-192.168.2.100}" >/dev/null 2>&1; then
echo "OK — 当前臂 ${SITE_ROBOT_SN:-} @ ${SITE_ROBOT_IP} 可达。RDK 用 SN 连接。"
echo " 063989 @ 192.168.2.101；063991 @ 192.168.2.100。"
else
echo "当前臂 ${SITE_ROBOT_IP:-192.168.2.100} 仍不通。检查："
echo " 1) User1 网线 ↔ 交换机 ↔ 控制柜网口（灯亮）"
echo " 2) 示教器 About 里的 IPv4（出厂多为 192.168.2.100）"
echo " 3) Remote / Ethernet RDK 已开"
exit 2
fi
scripts/gen_site_example.py
+139
View File
@@ -0,0 +1,139 @@
#!/usr/bin/env python3
"""从 ``config/site.yaml`` 生成 ``config/site.example.yaml``。
为什么要有这个脚本
------------------
``config/site.yaml`` 是现场唯一权威配置，也是全项目注释最全的文件。
``config/site.example.yaml`` 供新环境部署时复制参考。
过去两者是各写各的，结果是模板长期落后于现场配置：分析时发现模板缺了
114 项（``servo.*``、``hole_detect.stage2.*``、``sim.*`` 等），新人照着模板
配完会发现一堆功能不工作。
本脚本把「模板」变成「site.yaml 的匿名化副本」，于是模板永远不会漂移：
改完 site.yaml 重跑本脚本即可。
做什么
------
读 ``config/site.yaml``，把现场专属的值替换成占位值，其余**逐字节保留**
（包括全部中文注释与排版），然后写入 ``config/site.example.yaml``。
用法::
python3 scripts/gen_site_example.py # 生成 / 更新模板
python3 scripts/gen_site_example.py --check # 只检查是否已同步（CI / 测试用）
``--check`` 在不一致时以退出码 1 结束，方便接进测试。
"""
from __future__ import annotations
import argparse
import re
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
SITE = ROOT / "config" / "site.yaml"
EXAMPLE = ROOT / "config" / "site.example.yaml"
#: 生成文件顶部的说明。生成物是只读产物，手改会在下次生成时丢失。
HEADER = """\
# =============================================================================
# site.example.yaml — 新环境部署模板
#
# ！本文件由 scripts/gen_site_example.py 从 config/site.yaml 自动生成，请勿手改。
# 要改模板内容，请改 config/site.yaml（它是唯一权威）后重跑：
# python3 scripts/gen_site_example.py
# 或在改完 site.yaml 后跑一次以确认模板同步：
# python3 scripts/gen_site_example.py --check
#
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
# 现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
#
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
# 部署时请改成实际用户的 home 目录。
# =============================================================================
"""
#: 逐字替换。长串必须排在前面，否则会被短串规则截断。
LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
# 头部那句"从 example 复制"与本文件的实际流程相反，改掉。
(
"# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制）",
"# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制而来）",
),
# 用户名匿名化。放在其它规则之前，避免路径被二次处理。
("/home/cc", "/home/USER"),
# 注释里裸写的机械臂编号（无引号，正则抓不到）。
(
"# User1=enp4s0 本机 192.168.2.10 ↔ 063989=.101 / 063991=.100",
"# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100",
),
]
#: 正则替换。按顺序应用。
REGEX_REPLACEMENTS: list[tuple[str, str]] = [
# 机械臂序列号：Rizon4-063989 -> Rizon4-XXXXXX
(r'"(Rizon4)-\d+"', r'"\1-XXXXXX"'),
# IP 地址（仅带引号的配置值；注释里的裸 IP 由上面的字面替换处理）
(r'"192\.168\.\d{1,3}\.\d{1,3}"', '"192.168.2.XXX"'),
# 8 位及以上的纯数字字符串 = 设备序列号（相机 12 位 / 夹爪 8 位）。
# 不会误伤别的值：本文件里没有其它 8 位以上的数字字符串字面量。
(r'"\d{8,}"', '"<序列号>"'),
# USB 视频设备的 by-path 稳定路径：端口号随机器而变。
(r"(usb-0:)\d+\.\d+\.\d+", r"\1X.Y.Z"),
]
def render(site_text: str) -> str:
"""Turn the live config text into the anonymised template text."""
out = site_text
for old, new in LITERAL_REPLACEMENTS:
out = out.replace(old, new)
for pattern, replacement in REGEX_REPLACEMENTS:
out = re.sub(pattern, replacement, out)
return HEADER + out
def read_site() -> str:
if not SITE.is_file():
print(f"错误: 找不到 {SITE}", file=sys.stderr)
raise SystemExit(2)
return SITE.read_text(encoding="utf-8")
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(
description="从 config/site.yaml 生成 config/site.example.yaml",
)
parser.add_argument(
"--check",
action="store_true",
help="只检查模板是否与 site.yaml 同步；不同步则以退出码 1 结束（不写文件）",
)
args = parser.parse_args(argv)
expected = render(read_site())
if args.check:
actual = EXAMPLE.read_text(encoding="utf-8") if EXAMPLE.is_file() else ""
if actual == expected:
print(f"模板已同步: {EXAMPLE}")
return 0
print(
"模板与 config/site.yaml 不同步。\n"
" config/site.yaml 改动后，请重跑: python3 scripts/gen_site_example.py",
file=sys.stderr,
)
return 1
EXAMPLE.parent.mkdir(parents=True, exist_ok=True)
EXAMPLE.write_text(expected, encoding="utf-8")
print(f"已生成 {EXAMPLE}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
scripts/go_home.sh
Executable
+283
View File
@@ -0,0 +1,283 @@
#!/usr/bin/env bash
# Move Rizon4 to a named pose (default: home) and FORCE stop.
#
# Critical: visual_servo.launch / servo_node must be killed first, otherwise
# they keep streaming JointTrajectory and the arm never settles.
#
# Usage:
# bash ~/ros2/scripts/go_home.sh
# bash ~/ros2/scripts/go_home.sh --pose ready --speed 0.3
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
POSE="${POSE:-}"
SPEED="${SPEED:-}"
KEEP_SERVO=0
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
usage() {
cat <<'EOF'
Move arm to a named pose (default: home), then hold with zero velocity.
Default: kill perception/servo/launch first (required to stop shaking).
Options:
--pose NAME home | ready | inspect | pos_left
--speed FLOAT velocity_scale (0,1] default 0.25
--arm 89|91 与 run_real 同一臂
--keep-servo do NOT kill servo (arm may keep shaking)
-h, --help
EOF
}
while [[ $# -gt 0 ]]; do
case "$1" in
--pose) POSE="${2:?}"; shift 2 ;;
--speed) SPEED="${2:?}"; shift 2 ;;
--keep-servo) KEEP_SERVO=1; shift ;;
--stop-track) shift ;; # legacy
-h|--help) usage; exit 0 ;;
*) echo "Unknown: $1" >&2; usage >&2; exit 1 ;;
esac
done
if ! command -v ros2 >/dev/null 2>&1 || [[ -z "${ROS_DISTRO:-}" ]]; then
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
fi
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
POSE="${POSE:-${SITE_GO_HOME_POSE:-home}}"
SPEED="${SPEED:-${SITE_GO_HOME_SPEED:-0.25}}"
kill_streamers() {
echo "=== killing visual-servo streamers ==="
# Prefer disable control before kill (if node still up)
ros2 param set /servo_node enable_control false >/dev/null 2>&1 || true
# Kill launch parent first, then executables (SIGTERM then SIGKILL)
local patterns=(
'visual_servo.launch.py'
'/lib/arm_application/servo_node'
'/lib/arm_application/perception_node'
)
local p
for p in "${patterns[@]}"; do
if pgrep -f "$p" >/dev/null 2>&1; then
echo " TERM $p"
pkill -TERM -f "$p" 2>/dev/null || true
fi
done
sleep 1.0
for p in "${patterns[@]}"; do
if pgrep -f "$p" >/dev/null 2>&1; then
echo " KILL $p"
pkill -KILL -f "$p" 2>/dev/null || true
fi
done
sleep 0.5
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
echo "ERROR: servo_node still alive — refuse to move (would keep shaking)" >&2
pgrep -af '/lib/arm_application/servo_node' >&2 || true
exit 2
fi
echo " servo_node gone OK"
}
print_joints() {
python3 <<'PY'
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
rclpy.init()
n = Node("go_home_joints")
box = [None]
n.create_subscription(JointState, "joint_states", lambda m: box.__setitem__(0, m), 10)
for _ in range(40):
rclpy.spin_once(n, timeout_sec=0.05)
if box[0] is not None:
break
if box[0] is None:
print(" (no joint_states)")
else:
m = dict(zip(box[0].name, box[0].position))
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
for i in range(1, 8):
k = f"{__import__('os').environ.get('SITE_JOINT_PREFIX','Rizon4-063989_')}joint{i}"
if k in m:
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
n.destroy_node(); rclpy.shutdown()
PY
}
# Strong settle via controller ACTION (not topic — matches JTC reliably)
settle_hold_action() {
local pose="$1"
python3 - "$pose" <<'PY'
import sys, time
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
POSES = {
"home": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
"ready": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
"inspect": [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
"pos_left": [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
}
name = sys.argv[1]
target = POSES.get(name)
if target is None:
raise SystemExit(f"unknown pose {name}")
import os
PREFIX = os.environ.get("SITE_JOINT_PREFIX", "Rizon4-063989_")
JOINTS = [f"{PREFIX}joint{i}" for i in range(1, 8)]
rclpy.init()
n = Node("go_home_hold_action")
js_box = [None]
n.create_subscription(JointState, "joint_states", lambda m: js_box.__setitem__(0, m), 10)
cli = ActionClient(n, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory")
if not cli.wait_for_server(timeout_sec=5.0):
print(" WARN: JTC action not available — skip action hold")
n.destroy_node(); rclpy.shutdown(); raise SystemExit(0)
def spin_until(pred, timeout=3.0):
t0 = time.monotonic()
while time.monotonic() - t0 < timeout:
rclpy.spin_once(n, timeout_sec=0.05)
if pred():
return True
return False
spin_until(lambda: js_box[0] is not None, 3.0)
# Two-point: current -> target (or target hold), BOTH with zero velocity
q_now = list(target)
if js_box[0] is not None:
m = dict(zip(js_box[0].name, js_box[0].position))
try:
q_now = [float(m[j]) for j in JOINTS]
except KeyError:
pass
def send_hold(q, T=0.40):
traj = JointTrajectory()
traj.joint_names = list(JOINTS)
# start
p0 = JointTrajectoryPoint()
p0.positions = [float(v) for v in q]
p0.velocities = [0.0] * 7
p0.accelerations = [0.0] * 7
p0.time_from_start.sec = 0
p0.time_from_start.nanosec = 50_000_000
# end same pose, still zero vel (pin)
p1 = JointTrajectoryPoint()
p1.positions = [float(v) for v in q]
p1.velocities = [0.0] * 7
p1.accelerations = [0.0] * 7
ns = int(round(T * 1e9))
p1.time_from_start.sec = ns // 1_000_000_000
p1.time_from_start.nanosec = ns % 1_000_000_000
traj.points = [p0, p1]
goal = FollowJointTrajectory.Goal()
goal.trajectory = traj
fut = cli.send_goal_async(goal)
spin_until(lambda: fut.done(), 3.0)
if not fut.done():
print(" hold goal send timeout")
return False
gh = fut.result()
if gh is None or not gh.accepted:
print(" hold goal rejected")
return False
rfut = gh.get_result_async()
spin_until(lambda: rfut.done(), T + 3.0)
return True
# 1) snap toward named pose with zero end vel
print(f" action hold → {name} (zero vel)")
send_hold(target, T=0.60)
time.sleep(0.4)
# 2–4) re-pin measured pose several times (align JTC open-loop cmd to encoder)
for i in range(3):
spin_until(lambda: js_box[0] is not None, 1.0)
if js_box[0] is None:
break
m = dict(zip(js_box[0].name, js_box[0].position))
q_pin = [float(m[j]) for j in JOINTS]
print(f" action re-pin #{i+1} @ measured joints (zero vel)")
send_hold(q_pin, T=0.45)
time.sleep(0.25)
# Wait until |qd| small
print(" waiting for joints to stop ...")
t0 = time.monotonic()
while time.monotonic() - t0 < 5.0:
rclpy.spin_once(n, timeout_sec=0.05)
if js_box[0] is None or not js_box[0].velocity:
continue
vmap = dict(zip(js_box[0].name, js_box[0].velocity))
vmax = max(abs(float(vmap.get(j, 0.0))) for j in JOINTS)
if vmax < 0.015:
print(f" settled |qd|_max={vmax:.4f} rad/s")
break
else:
print(" WARN: still some residual velocity")
n.destroy_node()
rclpy.shutdown()
PY
}
if [[ "${KEEP_SERVO}" -eq 0 ]]; then
kill_streamers
else
echo "WARNING: --keep-servo — shaking likely"
fi
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: /set_named_pose missing — start run_real.sh" >&2
exit 1
fi
echo "=== joints before ==="
print_joints
echo "=== move: ${POSE} speed=${SPEED} ==="
RESP="$(
ros2 service call /set_named_pose arm_msgs/srv/SetNamedPose \
"{pose_name: '${POSE}', velocity_scale: ${SPEED}, acceleration_scale: 0.0}" 2>&1
)"
echo "${RESP}"
if ! echo "${RESP}" | grep -q 'success=True'; then
echo "ERROR: move failed" >&2
exit 1
fi
WAIT_SEC="$(python3 -c "print(max(2.5, 1.0 / max(float('${SPEED}'), 0.05) + 2.0))")"
echo "Waiting ${WAIT_SEC}s ..."
sleep "${WAIT_SEC}"
echo "=== settle (action, vel=0) ==="
settle_hold_action "${POSE}"
# Final check: servo must still be dead
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
echo "WARNING: servo_node came back — killing again"
kill_streamers
settle_hold_action "${POSE}"
fi
echo "=== joints after ==="
print_joints
echo "Done. Arm should be still."
echo "Tracking later: bash ~/ros2/scripts/run_visual_servo_rt.sh --control"
scripts/install_rdk_offline.sh
Executable
+104
View File
@@ -0,0 +1,104 @@
#!/usr/bin/env bash
# Offline-friendly Flexiv RDK v1.9.1 install for ROS (humble-v1 / flexiv_hardware).
# CMakeLists already supports FLEXIV_RDK_OFFLINE_LIB / vendor/rdk_libs/.
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
RDK_SRC="${RDK_SRC:-$ROOT/src/flexiv_rdk_v1}"
PREFIX="${PREFIX:-$HOME/flexiv_install}"
LIB_NAME="libflexiv_rdk.x86_64-linux-gnu.a"
EXPECTED_HASH="$(tr -d '[:space:]' < "${RDK_SRC}/lib/${LIB_NAME}.sha256")"
VENDOR_LIB="${ROOT}/vendor/rdk_libs/${LIB_NAME}"
JOBS="${JOBS:-$(nproc)}"
# Prefer conda ros_flexiv toolchain (cmake/g++ often only there)
if ! command -v cmake >/dev/null 2>&1; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh" 2>/dev/null \
|| source "${HOME}/miniconda3/etc/profile.d/conda.sh" 2>/dev/null \
|| true
conda activate "${CONDA_ENV:-ros_flexiv}" 2>/dev/null || true
fi
if ! command -v cmake >/dev/null 2>&1; then
echo "ERROR: cmake not found. Activate ros_flexiv (or install cmake) first."
exit 1
fi
echo "Using cmake: $(command -v cmake) ($(cmake --version | head -1))"
mkdir -p "${ROOT}/vendor/rdk_libs" "${PREFIX}"
if [[ -f "${VENDOR_LIB}" ]] && [[ "$(sha256sum "${VENDOR_LIB}" | awk '{print $1}')" == "${EXPECTED_HASH}" ]]; then
echo "OK: ${VENDOR_LIB}"
else
echo "Need ${LIB_NAME} (SHA256 ${EXPECTED_HASH})"
base="https://github.com/flexivrobotics/flexiv_rdk/releases/download/v1.9.1/${LIB_NAME}"
ok=0
for url in \
"${base}" \
"https://ghproxy.net/${base}" \
"https://gh.ddlc.top/${base}" \
"https://hub.gitmirror.com/${base}" \
"https://cf.ghproxy.cc/${base}"
do
echo "TRY ${url}"
if curl -fL --connect-timeout 15 --max-time 120 -o "${VENDOR_LIB}.part" "${url}" \
&& [[ "$(sha256sum "${VENDOR_LIB}.part" | awk '{print $1}')" == "${EXPECTED_HASH}" ]]; then
mv "${VENDOR_LIB}.part" "${VENDOR_LIB}"
ok=1
break
fi
rm -f "${VENDOR_LIB}.part"
done
if [[ "${ok}" -ne 1 ]]; then
cat <<EOF
FAILED to download ${LIB_NAME}.
Manual (any PC with GitHub access):
wget -O ${LIB_NAME} '${base}'
sha256sum ${LIB_NAME} # must be ${EXPECTED_HASH}
scp ${LIB_NAME} $(hostname):${VENDOR_LIB}
Then re-run: $0
EOF
exit 1
fi
fi
export FLEXIV_RDK_OFFLINE_LIB="${VENDOR_LIB}"
echo "Install deps into ${PREFIX} (may still need git mirrors)..."
DEP_SCRIPT=""
for s in \
"${RDK_SRC}/thirdparty/build_and_install_dependencies_not_in_ros2.sh" \
"${RDK_SRC}/thirdparty/build_and_install_dependencies.sh"
do
[[ -f "$s" ]] && DEP_SCRIPT="$s" && break
done
if [[ -n "${DEP_SCRIPT}" ]]; then
bash "${DEP_SCRIPT}" "${PREFIX}" "${JOBS}" || {
echo "WARN: deps script failed. If Eigen/spdlog/Fast-DDS/RBDyn already in ${PREFIX}, continue."
}
else
echo "WARN: no dependency script found"
fi
rm -rf "${RDK_SRC}/build"
mkdir -p "${RDK_SRC}/build" && cd "${RDK_SRC}/build"
cmake .. -DCMAKE_BUILD_TYPE=Release \
-DCMAKE_INSTALL_PREFIX="${PREFIX}" \
-DCMAKE_PREFIX_PATH="${PREFIX}"
cmake --build . --target install -j "${JOBS}"
cat <<EOF
RDK installed to ${PREFIX}
Next:
conda activate ros_flexiv
cd ${ROOT}
colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=${PREFIX} \\
--packages-up-to flexiv_bringup flexiv_hardware arm_bringup
Python (already ready, no GitHub):
/home/cc/rdk_py310/bin/python
bash ${ROOT}/scripts/test_rdk_python.sh
EOF
scripts/lib/export_site_env.py
+563
View File
@@ -0,0 +1,563 @@
#!/usr/bin/env python3
"""Load ~/ros2/config/site.yaml and print bash export lines or JSON.
Usage:
eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)"
python3 ~/ros2/scripts/lib/export_site_env.py --json
python3 ~/ros2/scripts/lib/export_site_env.py --print
"""
from __future__ import annotations
import argparse
import json
import os
import sys
from pathlib import Path
try:
import yaml
except ImportError:
yaml = None # type: ignore
def _root() -> Path:
return Path(__file__).resolve().parents[2]
def site_path() -> Path:
env = os.environ.get("SITE_CONFIG", "").strip()
if env:
return Path(env).expanduser()
return _root() / "config" / "site.yaml"
def load_site(path: Path | None = None) -> dict:
path = path or site_path()
if not path.is_file():
raise FileNotFoundError(f"site config not found: {path}")
text = path.read_text(encoding="utf-8")
if yaml is None:
raise RuntimeError("PyYAML required: pip/conda install pyyaml")
data = yaml.safe_load(text) or {}
if not isinstance(data, dict):
raise ValueError(f"invalid site.yaml: {path}")
return data
def list_robots(data: dict) -> list[dict]:
"""Catalog entries from robot.robots (list or SN-keyed map)."""
raw = (data.get("robot") or {}).get("robots") or []
out: list[dict] = []
if isinstance(raw, dict):
for sn, item in raw.items():
row = dict(item or {}) if isinstance(item, dict) else {}
row.setdefault("sn", str(sn))
out.append(row)
return out
if isinstance(raw, list):
for item in raw:
if isinstance(item, dict) and str(item.get("sn") or "").strip():
out.append(dict(item))
return out
# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
_ARM_ALIASES = {
"89": "Rizon4-063989",
"left": "Rizon4-063989",
"左": "Rizon4-063989",
"左臂": "Rizon4-063989",
"左臂89": "Rizon4-063989",
"91": "Rizon4-063991",
"right": "Rizon4-063991",
"右": "Rizon4-063991",
"右臂": "Rizon4-063991",
"右臂91": "Rizon4-063991",
}
_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
def _deep_merge(base: dict, overlay: dict) -> dict:
out = dict(base)
for k, v in overlay.items():
if isinstance(out.get(k), dict) and isinstance(v, dict):
out[k] = _deep_merge(out[k], v)
else:
out[k] = v
return out
def normalize_robot_sn(data: dict, want: str) -> str:
"""Map 89/91/left/right/label → full Flexiv SN."""
w = (want or "").strip()
if not w:
return w
mapped = _ARM_ALIASES.get(w) or _ARM_ALIASES.get(w.lower())
if mapped:
return mapped
for item in list_robots(data):
sn = str(item.get("sn") or "").strip()
label = str(item.get("label") or "").strip()
arm = str(item.get("arm") or "").strip().lower()
if w == sn or w == label or w.lower() == arm:
return sn
if sn.endswith(w) and w.isdigit() and len(w) >= 2:
return sn
return w
def resolve_robot(data: dict, sn: str | None = None) -> dict:
"""Active arm: ROBOT_SN env overrides robot.sn; catalog fills ip/iface/frames."""
robot = dict(data.get("robot") or {})
catalog = list_robots(data)
want = normalize_robot_sn(
data,
sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
)
match: dict = {}
for item in catalog:
if str(item.get("sn") or "").strip() == want:
match = dict(item)
break
out = {k: v for k, v in robot.items() if k != "robots"}
out.update({k: v for k, v in match.items() if k != "robots"})
out["sn"] = want
if not str(out.get("arm") or "").strip():
if "063989" in want:
out["arm"] = "left"
elif "063991" in want:
out["arm"] = "right"
if not str(out.get("joint_prefix") or "").strip() and want:
out["joint_prefix"] = f"{want}_"
if not str(out.get("base_frame") or "").strip() and want:
out["base_frame"] = f"{want}_base_link"
if not str(out.get("ee_frame") or "").strip() and want:
out["ee_frame"] = f"{want}_flange"
net = data.get("network") or {}
if not str(out.get("iface") or "").strip():
out["iface"] = str(net.get("prefer_iface") or "")
if not str(out.get("ip") or "").strip():
out["ip"] = "192.168.100.1"
if not str(out.get("pc_ip") or "").strip():
out["pc_ip"] = "192.168.100.10"
return out
def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
"""Merge robots[] camera/gripper/paths/servo onto site defaults for the active SN."""
robot = resolve_robot(data, sn)
merged = dict(data)
for key in _ARM_SECTION_KEYS:
base = merged.get(key) if isinstance(merged.get(key), dict) else {}
ov = robot.get(key) if isinstance(robot.get(key), dict) else {}
merged[key] = _deep_merge(dict(base or {}), dict(ov or {}))
return merged, robot
def camera_ns(data: dict) -> str:
"""Topic namespace for the active wrist camera (d405_left / d405_right)."""
cam = data.get("camera") or {}
name = str(cam.get("name") or "d405").strip().strip("/")
return name or "d405"
def camera_frames(data: dict) -> tuple[str, str, str]:
"""(namespace, optical_frame, depth_topic) for the active arm."""
ns = camera_ns(data)
yolo = data.get("yolo") or {}
frame = str(yolo.get("camera_frame") or "").strip()
depth = str(yolo.get("depth_topic") or "").strip()
if frame in ("", "d405_color_optical_frame"):
frame = f"{ns}_color_optical_frame"
if depth in ("", "/d405/aligned_depth_to_color/image_raw"):
depth = f"/{ns}/aligned_depth_to_color/image_raw"
return ns, frame, depth
def flatten(data: dict) -> dict:
"""Flat SITE_* env map for bash scripts."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
cam = data.get("camera") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
home = data.get("go_home") or {}
net = data.get("network") or {}
he = data.get("handeye") or {}
grip = data.get("gripper") or {}
named_poses = str(
robot.get("named_poses_file")
or paths.get("named_poses_file")
or str(_root() / "src" / "arm_control" / "config" / "named_poses.yaml")
)
weights = str(paths.get("yolo_weights") or "")
# Resolve active.pt symlink to real path for display
try:
weights_resolved = str(Path(weights).expanduser().resolve())
except Exception:
weights_resolved = weights
out = {
"SITE_CONFIG": str(site_path()),
"SITE_ROS2_ROOT": str(_root()),
"SITE_ROBOT_SN": str(robot.get("sn") or "Rizon4-063989"),
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
"SITE_BASE_FRAME": str(robot.get("base_frame") or ""),
"SITE_EE_FRAME": str(robot.get("ee_frame") or ""),
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
"SITE_NAMED_POSES_FILE": named_poses,
"SITE_ROBOT_CATALOG": ",".join(
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
),
"SITE_YOLO_WEIGHTS": weights,
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
"SITE_HANDEYE_WAYPOINTS": str(
paths.get("handeye_waypoints")
or str(_root() / "src" / "arm_application" / "config" / "handeye_poses.yaml")
),
"SITE_HANDEYE_CALIBRATE": str(
paths.get("handeye_calibrate")
or str(_root() / "src" / "arm_application" / "config" / "handeye_calibrate.yaml")
),
"SITE_HANDEYE_TOUR": str(
paths.get("handeye_tour")
or str(_root() / "src" / "arm_application" / "config" / "handeye_waypoints.yaml")
),
"SITE_HANDEYE_WORKDIR": str(
paths.get("handeye_workdir") or "/tmp/handeye_calib"
),
"SITE_HANDEYE_FILE": str(
paths.get("handeye_file")
or str(_root() / "src" / "arm_application" / "config" / "handeye_result.yaml")
),
"SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
"SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 11)),
"SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 16)),
"SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.011)),
"SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
"SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
"SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),
"SITE_HANDEYE_SETTLE_QD": str(float(he.get("settle_qd_max") or 0.015)),
"SITE_HANDEYE_SETTLE_STABLE": str(float(he.get("settle_stable_sec") or 0.6)),
"SITE_HANDEYE_SETTLE_TO": str(float(he.get("settle_timeout_sec") or 8.0)),
"SITE_HANDEYE_PIN_HOLDS": str(int(he.get("pin_holds") if he.get("pin_holds") is not None else 2)),
"SITE_HANDEYE_PIN_SEC": str(float(he.get("pin_hold_sec") or 0.45)),
"SITE_HANDEYE_SKIP_UNSTABLE": (
"true" if he.get("skip_unstable_poses", True) else "false"
),
"SITE_FASTDDS_PROFILE": str(paths.get("fastdds_profile") or ""),
"SITE_CAM_NAME": camera_ns(data),
"SITE_CAM_SERIAL": str(cam.get("serial") or ""),
"SITE_CAM_WIDTH": str(int(cam.get("width") or 848)),
"SITE_CAM_HEIGHT": str(int(cam.get("height") or 480)),
"SITE_CAM_FPS": str(int(cam.get("fps") or 30)),
"SITE_CAM_ENABLE_DEPTH": "1" if cam.get("enable_depth", True) else "0",
"SITE_CAM_ENABLE_INFRA": "1" if cam.get("enable_infra", True) else "0",
"SITE_CAM_PREVIEW": "1" if cam.get("preview", True) else "0",
"SITE_CAM_PREVIEW_STEREO": "1" if cam.get("preview_stereo", True) else "0",
"SITE_CAM_OVERLAY_HOLD": str(float(cam.get("overlay_hold_sec") or 1.5)),
"SITE_CAM_USE_SITE_RES": "1" if cam.get("use_site_resolution", True) else "0",
"SITE_CAM_AUTO_EXPOSURE": "1" if cam.get("auto_exposure", True) else "0",
"SITE_CAM_AE_LIMIT_US": str(float(cam.get("ae_limit_us") if cam.get("ae_limit_us") is not None else 4000)),
"SITE_CAM_EMITTER": "1" if cam.get("emitter_enabled", False) else "0",
"SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)),
"SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)),
"SITE_CAM_GAIN": str(float(cam.get("gain") or 0)),
"SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""),
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or "352122274408"),
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or "260522274442"),
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.35),
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 416)),
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
"SITE_YOLO_MIN_CONF": str(yolo.get("min_conf") or 0.35),
"SITE_GO_HOME_POSE": str(home.get("pose") or "home"),
"SITE_GO_HOME_SPEED": str(home.get("speed") or 0.25),
"SITE_NET_IFACE": str(robot.get("iface") or net.get("prefer_iface") or ""),
"SITE_SERVO_Z_REF": str(servo.get("z_ref_m") or 0.40),
"SITE_EE_X_M": str(float(servo.get("ee_x_m") or 0.25)),
"SITE_EE_Y_M": str(float(servo.get("ee_y_m") or 0.0)),
"SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)),
"SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3),
"SITE_CAMERA_FRAME": camera_frames(data)[1],
"SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"),
"SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"),
"SITE_GRIP_SN": str(grip.get("sn") or ""),
"SITE_GRIP_BACKEND": str(grip.get("backend") or "usb"),
"SITE_GRIP_DEVICE": str(grip.get("device_name") or "GripperDahuanModbus"),
"SITE_GRIP_FORCE_N": str(float(grip.get("force_n") or 50.0)),
"SITE_GRIP_VEL_MPS": str(float(grip.get("vel_mps") or 0.05)),
"SITE_GRIP_PORT": str(grip.get("port") or "/dev/ttyUSB0"),
"SITE_GRIP_BAUD": str(int(grip.get("baud") or 115200)),
"SITE_GRIP_SLAVE": str(int(grip.get("slave_id") or 1)),
"SITE_GRIP_STROKE_MM": str(float(grip.get("stroke_mm") or 20.0)),
"SITE_GRIP_FORCE": str(int(grip.get("force_pct") or 50)),
"SITE_GRIP_SPEED": str(int(grip.get("speed_pct") or 50)),
"SITE_GRIP_AUTO_INIT": "true" if grip.get("auto_init", True) else "false",
"SITE_GRIP_RS485_RTS": str(grip.get("rs485_rts") or ""),
"SITE_HOLE_BACKEND": str((data.get("hole_detect") or {}).get("backend") or "two_stage"),
"SITE_HOLE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("weights")
or paths.get("hole_weights")
or str(_root() / "models" / "hole_insert" / "best.pt")
),
"SITE_HOLE_WORKPIECE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("workpiece_weights")
or paths.get("hole_workpiece_weights")
or weights
),
"SITE_HOLE_CONFIG": str(
(data.get("hole_detect") or {}).get("config")
or (
str(_root() / "src" / "arm_application" / "config" / "hole_detect.yaml")
if str((data.get("hole_detect") or {}).get("backend") or "two_stage")
in ("two_stage", "hough_roi")
else str(_root() / "src" / "arm_application" / "config" / "hole_insert.yaml")
)
),
"SITE_TACTILE_BACKEND": str((data.get("tactile") or {}).get("backend") or "orisys"),
"SITE_TACTILE_CONFIG": str((data.get("tactile") or {}).get("config") or "dd03"),
"SITE_TACTILE_CONFIG_FILE": str(
(data.get("tactile") or {}).get("config_file")
or str(_root() / "src" / "arm_application" / "config" / "orisys" / "dd03.json")
),
"SITE_TACTILE_LEFT": str((data.get("tactile") or {}).get("left_video", 0)),
"SITE_TACTILE_RIGHT": str((data.get("tactile") or {}).get("right_video", 2)),
}
return out
def gripper_overrides(data: dict) -> dict:
"""ROS parameter dict for dh_pgea_node (DH Modbus, not Flexiv gripper)."""
data, _robot = apply_active_arm(data)
g = data.get("gripper") or {}
return {
"port": str(g.get("port") or "/dev/ttyUSB0"),
"baud": int(g.get("baud") or 115200),
"slave_id": int(g.get("slave_id") or 1),
"stroke_mm": float(g.get("stroke_mm") or 20.0),
"force_pct": int(g.get("force_pct") or 50),
"speed_pct": int(g.get("speed_pct") or 50),
"auto_init": bool(g.get("auto_init", True)),
"state_hz": float(g.get("state_hz") or 20.0),
"model": str(g.get("model") or "PGEA-100-20-W"),
"serial_number": str(g.get("sn") or ""),
"open_position": int(g.get("open_position") if g.get("open_position") is not None else 1000),
"close_position": int(g.get("close_position") if g.get("close_position") is not None else 0),
"rs485_rts": str(g.get("rs485_rts") or ""),
}
def perception_overrides(data: dict) -> dict:
"""ROS parameter dict for perception_node."""
data, _robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
cam_ns, cam_frame, depth_topic = camera_frames(data)
obj = str(yolo.get("object_frame") or "").strip()
if not obj or obj == "tracked_object":
arm = str((_robot or {}).get("arm") or "")
sn = str((_robot or {}).get("sn") or "")
if "063991" in sn or arm == "right":
obj = "tracked_object_91"
elif "063989" in sn or arm == "left":
obj = "tracked_object_89"
else:
obj = "tracked_object"
return {
"weights": str(paths.get("yolo_weights") or ""),
"conf": float(yolo.get("conf") or 0.10),
"iou": float(yolo.get("iou") or 0.45),
"imgsz": int(yolo.get("imgsz") or 416),
"device": str(yolo.get("device") or "0"),
"half_precision": bool(yolo.get("half_precision", True)),
"backend": str(yolo.get("backend") or "auto"),
"mp_infer": bool(yolo.get("mp_infer", True)),
"min_conf": float(yolo.get("min_conf") or 0.12),
"vision_hz": float(yolo.get("vision_hz") or 30.0),
"infer_max_width": int(yolo.get("infer_max_width") if yolo.get("infer_max_width") is not None else 0),
"box_ema_alpha": float(yolo.get("box_ema_alpha") or 0.85),
"miss_clear_frames": int(yolo.get("miss_clear_frames") or 12),
"overlay_hold_on_miss": bool(yolo.get("overlay_hold_on_miss", True)),
"jump_px": float(yolo.get("jump_px") or 280.0),
"jump_z_m": float(yolo.get("jump_z_m") or 0.20),
"z_ref_m": float(yolo.get("z_ref_m") or 0.22),
"min_depth_m": float(yolo.get("min_depth_m") or 0.04),
"max_depth_m": float(yolo.get("max_depth_m") or 0.55),
"work_z_max_m": float(yolo.get("work_z_max_m") or 0.50),
"hold_point_on_miss": bool(yolo.get("hold_point_on_miss", True)),
"miss_hold_point_frames": int(yolo.get("miss_hold_point_frames") or 2),
"meas_blend": float(yolo.get("meas_blend") or 0.65),
"z_alpha_min": float(yolo.get("z_alpha_min") or 0.10),
"z_alpha_max": float(yolo.get("z_alpha_max") or 0.60),
"z_alpha_gain": float(yolo.get("z_alpha_gain") or 2.0),
"camera_frame": cam_frame,
"object_frame": obj,
"depth_topic": depth_topic,
"camera_source": cam_ns,
"feature_mode": str(yolo.get("feature_mode") or "plane"),
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
}
def servo_overrides(data: dict) -> dict:
"""ROS parameter dict for servo_node."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
_ns, cam_frame, _depth = camera_frames(data)
out = {
"joint_prefix": str(robot.get("joint_prefix") or ""),
"base_frame": str(robot.get("base_frame") or ""),
"ee_frame": str(robot.get("ee_frame") or ""),
"handeye_file": str(paths.get("handeye_file") or ""),
"camera_frame": cam_frame,
"handeye_child_frame": cam_frame,
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
"z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40),
}
# Pass through remaining servo keys that match node params
keys = [
"control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
"max_extrapolate_sec", "predict_enable", "predict_sec",
"predict_v_max", "predict_jump_m",
"kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m",
"deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z",
"z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m",
"ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down",
"xy_track", "hover_offset_ee_xy_m", "arm_label",
"ee_hold_kp", "ee_w_max",
"lock_hold_sec",
"a_max", "max_qdot", "damping",
"vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
"min_cmd_delta_rad", "point_topic", "traj_topic",
"use_static_handeye_tf",
"workspace_xyz_min", "workspace_xyz_max",
]
for k in keys:
if k in servo:
out[k] = servo[k]
return out
def main() -> int:
ap = argparse.ArgumentParser()
ap.add_argument("--json", action="store_true")
ap.add_argument("--print", action="store_true", dest="do_print")
ap.add_argument("--config", default="")
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|full SN")
args = ap.parse_args()
if args.sn:
os.environ["ROBOT_SN"] = str(args.sn)
path = Path(args.config).expanduser() if args.config else site_path()
data = load_site(path)
flat = flatten(data)
if args.json:
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
return 0
if args.do_print:
print(f"site config: {path}")
print(
f" robot SN : {flat['SITE_ROBOT_SN']}"
f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}"
f" arm={flat.get('SITE_ROBOT_ARM') or '?'}"
)
print(f" catalog : {flat.get('SITE_ROBOT_CATALOG') or '(none)'}")
print(f" robot IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
print(f" frames : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
print(f" tool name : {flat.get('SITE_ROBOT_TOOL_NAME') or '(keep current)'}")
catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)]
if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns:
print(
f" WARNING: robot.sn {flat['SITE_ROBOT_SN']!r} is not in robots[]",
file=sys.stderr,
)
print(f" yolo weights : {flat['SITE_YOLO_WEIGHTS']}")
print(f" resolved : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
print(f" handeye : {flat['SITE_HANDEYE_FILE']}")
print(f" handeye wp : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" handeye dir : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" named poses : {flat.get('SITE_NAMED_POSES_FILE')}")
print(
" board : "
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
f"inner, square={flat['SITE_HANDEYE_SQUARE_M']} m"
)
print(
f" camera : serial={flat['SITE_CAM_SERIAL'] or '(empty)'} "
f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} "
f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} "
f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us "
f"gain={flat.get('SITE_CAM_GAIN')} "
f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}"
)
print(
f" catalog cam : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} "
f"D435={flat.get('SITE_D435_SERIAL')}"
)
print(
f" gripper : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
)
print(
f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} "
f"weights={flat.get('SITE_HOLE_WEIGHTS')}"
)
print(
f" tactile : backend={flat.get('SITE_TACTILE_BACKEND')} "
f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} "
f"cfg={flat.get('SITE_TACTILE_CONFIG')}"
)
print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}")
he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser()
if not he_path.is_file():
print(f" WARNING: hand-eye file missing: {he_path}", file=sys.stderr)
else:
try:
he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {}
except Exception:
he_data = {}
if isinstance(he_data, dict) and (
he_data.get("uncalibrated") or "T_4x4" not in he_data
):
print(
f" WARNING: {he_path.name} is not calibrated for this arm "
f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
file=sys.stderr,
)
if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
print(
" WARNING: 91 is using the LEFT wrist D405 serial — check robots[].camera.serial",
file=sys.stderr,
)
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
print(
" NOTE: 本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
file=sys.stderr,
)
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
if not w.is_file():
print(f" WARNING: weights file missing: {w}", file=sys.stderr)
return 2
return 0
# bash exports
for k, v in flat.items():
# escape for eval
vv = str(v).replace("'", "'\"'\"'")
print(f"export {k}='{vv}'")
return 0
if __name__ == "__main__":
raise SystemExit(main())
scripts/lib/load_site_config.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/usr/bin/env bash
# Load ~/ros2/config/site.yaml into SITE_* environment variables.
# Usage (from other scripts):
# source "${ROOT}/scripts/lib/load_site_config.sh"
_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}"
if [[ ! -f "${_SITE_FILE}" ]]; then
echo "[site] WARN: missing ${_SITE_FILE}" >&2
else
# shellcheck disable=SC1090
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"
export SITE_CONFIG="${_SITE_FILE}"
# Resolve 89/91/left/right aliases to the full Flexiv SN for RDK.
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then
export ROBOT_SN="${SITE_ROBOT_SN}"
fi
fi
scripts/lib/parse_arm_args.sh
+26
View File
@@ -0,0 +1,26 @@
#!/usr/bin/env bash
# Parse --sn / --arm from argv. Must be sourced BEFORE load_site_config.sh
# so ROBOT_SN is visible to export_site_env.py.
#
# source "${ROOT}/scripts/lib/parse_arm_args.sh"
# parse_arm_args "$@"
# set -- "${PARSE_ARM_REST[@]}"
#
# Aliases (89|91|left|right) are resolved in 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
}
scripts/list_realsense.sh
Executable
+21
View File
@@ -0,0 +1,21 @@
#!/usr/bin/env bash
# List plugged RealSense cameras (serials used in site.yaml).
set -eo pipefail
python3 - <<'PY'
import pyrealsense2 as rs
known = {
"352122274408": "left wrist D405 (arm 89, parallel gripper)",
"260522274442": "right wrist D405 (arm 91, rotary gripper)",
"262522073161": "overhead D435 (between arms, eye-to-hand)",
}
ds = list(rs.context().devices)
print(f"n={len(ds)}")
for d in ds:
sn = d.get_info(rs.camera_info.serial_number)
name = d.get_info(rs.camera_info.name)
usb = d.get_info(rs.camera_info.usb_type_descriptor)
fw = d.get_info(rs.camera_info.firmware_version)
role = known.get(sn, "(unassigned — add to site.yaml cameras:)")
print(f" {name:22} serial={sn} USB={usb} fw={fw}")
print(f" -> {role}")
PY
scripts/merge_handeye_poses.py
Executable
+143
View File
@@ -0,0 +1,143 @@
#!/usr/bin/env python3
"""Merge taught joint lines into handeye waypoints YAML.
Input (from handeye_teach / handeye_waypoints append file):
# pose 0
0.123456, -0.700000, ...
Target can be either:
- config/handeye/waypoints.yaml (/**: ros__parameters)
- handeye_calibrate.yaml (handeye_calibrate: ros__parameters)
Usage:
python3 scripts/merge_handeye_poses.py \\
~/ros2/config/handeye/taught_poses.txt \\
~/ros2/config/handeye/waypoints.yaml
# or via wrapper:
bash ~/ros2/scripts/run_handeye.sh merge
"""
from __future__ import annotations
import re
import sys
from pathlib import Path
def parse_poses(path: Path) -> list[list[float]]:
lines = path.read_text(encoding="utf-8").splitlines()
poses: list[list[float]] = []
buf: list[float] = []
num_re = re.compile(r"[-+]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][-+]?\d+)?")
for raw in lines:
line = raw.split("#", 1)[0].strip()
if not line:
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints, got {len(buf)}: {buf}")
poses.append(buf)
buf = []
continue
vals = [float(x) for x in num_re.findall(line)]
if not vals:
continue
if len(vals) == 7:
poses.append(vals)
elif len(vals) < 7:
buf.extend(vals)
if len(buf) > 7:
raise ValueError(f"Too many joint values in line: {line!r}")
else:
poses.append(vals[:7])
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints at EOF, got {len(buf)}")
poses.append(buf)
return poses
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
rows = [f"{indent}waypoint_positions: ["]
for i, q in enumerate(poses):
rows.append(f"{indent} # pose {i}")
s = ", ".join(f"{v:.6f}" for v in q) + ","
rows.append(f"{indent} {s}")
rows.append(f"{indent}]")
return "\n".join(rows)
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
# Detect indent of waypoint_positions (2 or 4 spaces typical)
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
indent = m_ind.group(1) if m_ind else " "
block = format_positions_block(poses, indent=indent)
pattern = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
re.MULTILINE,
)
# Also match a one-line empty list: waypoint_positions: []
one_line = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
re.MULTILINE,
)
if pattern.search(text):
new_text = pattern.sub(block, text, count=1)
elif one_line.search(text):
new_text = one_line.sub(block, text, count=1)
else:
raise RuntimeError("waypoint_positions block not found in YAML")
# Keep num_poses in sync when present
n = len(poses)
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
new_text = re.sub(
r"^(\s*num_poses:\s*)\d+",
rf"\g<1>{n}",
new_text,
count=1,
flags=re.MULTILINE,
)
else:
# Insert num_poses just before waypoint_positions
new_text = new_text.replace(
block,
f"{indent}num_poses: {n}\n{block}",
1,
)
yaml_path.write_text(new_text, encoding="utf-8")
def main(argv: list[str]) -> int:
if len(argv) != 3:
print(
"Usage: merge_handeye_poses.py <taught_poses.txt> <waypoints.yaml>",
file=sys.stderr,
)
return 1
src = Path(argv[1]).expanduser()
dst = Path(argv[2]).expanduser()
if not src.is_file():
print(f"Not found: {src}", file=sys.stderr)
return 2
if not dst.is_file():
print(f"Not found: {dst}", file=sys.stderr)
return 3
poses = parse_poses(src)
if len(poses) < 5:
print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
return 4
merge(dst, poses)
print(f"Merged {len(poses)} poses -> {dst}")
return 0
if __name__ == "__main__":
raise SystemExit(main(sys.argv))
scripts/rdk_drag_bridge.py
Executable
+329
View File
@@ -0,0 +1,329 @@
#!/usr/bin/env python3
"""RDK Free-Drive bridge (must run under rdk_py310 / Python 3.10).
Why soft-impedance drag
-----------------------
Under AUTO (Remote) the Studio Free Drive button is unavailable (remote owns
the robot). RDK FloatingJoint/FloatingCartesian often need an extra license.
So we default to soft NRT_JOINT_IMPEDANCE: low stiffness + target tracks
measured joints → arm can be pushed by hand while Remote stays on.
IMPORTANT: flexivrdk.Robot is NOT thread-safe. All robot API calls (including
the drag loop) share one lock. Joint reads during drag use a cache so Enter
does not race Stop()/SendJointPosition.
Line protocol (JSON on stdout only; RDK logs → stderr):
{"cmd":"ping"}
{"cmd":"connect","sn":"Rizon4-063989"}
{"cmd":"drag_on","mode":"soft"} # soft | floating
{"cmd":"drag_off"}
{"cmd":"joints"}
{"cmd":"quit"}
"""
from __future__ import annotations
import json
import sys
import threading
import time
from typing import List, Optional
_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 group {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 loop: {exc}')
time.sleep(0.15)
# ~20 Hz — keep lock released most of the time
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('soft loop still alive after join (will idle anyway)')
self._thread = None
def _ensure_operational(robot) -> Optional[str]:
try:
if robot.fault():
robot.ClearFault()
time.sleep(0.5)
if not robot.operational():
for fn in ('ServoOn', 'Enable'):
if hasattr(robot, fn):
try:
getattr(robot, fn)()
break
except Exception:
pass
t0 = time.monotonic()
while not robot.operational() and time.monotonic() - t0 < 10:
time.sleep(0.2)
if not robot.operational():
return (
'Robot not operational (fault/E-Stop?). '
'Clear fault on pendant, release E-Stop, AUTO Remote.'
)
except Exception as exc:
return str(exc)
return None
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
"""Switch to IDLE without blocking the stdin loop forever."""
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 timed out — stdin loop continues')
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'bad 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', 'Rizon4-063989'))
iface = str(msg.get('iface') or '')
robot = None
last_err = None
attempts = []
if iface:
attempts.extend([(sn, [iface]), (sn, [iface], True)])
attempts.append((sn,))
for args in attempts:
try:
robot = flexivrdk.Robot(*args)
log(f'connected {sn} args={args!r}')
last_err = None
break
except TypeError:
continue
except Exception as exc:
last_err = exc
log(f'connect {args!r}: {exc}')
robot = None
if robot is None:
reply({'ok': False, 'error': str(last_err or 'connect failed')})
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': 'not connected'})
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 failed, fallback 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('soft drag ON')
reply({
'ok': True,
'drag': True,
'mode': 'soft',
'hint': 'Push the arm gently; low joint impedance. Enter=photo (s=hold).',
})
except Exception as exc:
reply({'ok': False, 'error': f'soft drag failed: {exc}'})
elif cmd == 'drag_off':
log('drag_off requested')
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 done')
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': 'no joint cache'})
continue
cached_q = q
reply({'ok': True, 'q': q, 'drag': drag_on})
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'unknown cmd {cmd}'})
except Exception as exc: # noqa: BLE001
log(f'cmd={cmd} error: {exc}')
reply({'ok': False, 'error': str(exc)})
return 0
if __name__ == '__main__':
raise SystemExit(main())
scripts/rdk_gripper.py
Executable
+207
View File
@@ -0,0 +1,207 @@
#!/usr/bin/env python3
"""Control Studio device GripperDahuan / GripperDahuanModbus via Flexiv RDK.
Must run under RDK Python (rdk_py310). Uses a *lite* RDK instance so it can
share the robot with run_real.sh (ros2_control holds the normal RDK).
The controller talks RS485 itself (its own /dev/ttyUSB0), not the IPC serial.
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py init
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py open
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py close
"""
from __future__ import annotations
import argparse
import sys
import time
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts"))
from rdk_switch_tool import _iface_ipv4s # noqa: E402
def connect_lite(sn: str, iface: str = ""):
import flexivrdk
ips = _iface_ipv4s(iface)
if ips:
try:
return flexivrdk.Robot(sn, ips, True, True)
except TypeError:
pass
except RuntimeError as exc:
msg = str(exc).lower()
if "whitelist" not in msg and "filtered out" not in msg:
raise
return flexivrdk.Robot(sn, [], True, True)
def _fmt_params(p) -> str:
return (
f"width=[{p.min_width:.4f},{p.max_width:.4f}] m "
f"vel=[{p.min_vel:.4f},{p.max_vel:.4f}] m/s "
f"force=[{p.min_force:.1f},{p.max_force:.1f}] N"
)
def _fmt_states(s) -> str:
return f"width={s.width:.4f} m force={s.force:.2f} N moving={s.is_moving}"
def _clamp(val: float, lo: float, hi: float) -> float:
if lo > hi:
lo, hi = hi, lo
return max(lo, min(hi, val))
def _move(grip, params, width: float, vel: float, force: float) -> None:
vmin = float(params.min_vel)
vmax = float(params.max_vel)
if vmax <= 0:
vmax = 0.05
# Move velocity cannot be 0 (RDK).
vel = _clamp(vel, max(vmin, 1e-4), vmax)
force = _clamp(force, float(params.min_force), float(params.max_force))
width = _clamp(width, float(params.min_width), float(params.max_width))
print(f"Move width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N")
grip.Move(width, vel, force)
def main() -> int:
ap = argparse.ArgumentParser(description="Flexiv RDK gripper (GripperDahuanModbus)")
ap.add_argument("--sn", default="")
ap.add_argument("--iface", default="")
ap.add_argument("--device", default="", help="Studio Device name")
ap.add_argument("--force-n", type=float, default=0.0, help="force limit [N]; 0=mid")
ap.add_argument("--vel", type=float, default=0.0, help="speed [m/s]; 0=half max")
ap.add_argument(
"cmd",
nargs="?",
default="status",
choices=["list", "status", "init", "open", "close", "pos", "enable"],
)
ap.add_argument("value", nargs="?", default="", help="pos: meters or 0–1000")
args = ap.parse_args()
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
from export_site_env import load_site, resolve_robot # type: ignore
data = load_site()
robot_cfg = resolve_robot(data)
g = data.get("gripper") or {}
sn = (args.sn or robot_cfg.get("sn") or "").strip()
iface = (args.iface or robot_cfg.get("iface") or "").strip()
device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip()
if not sn:
print("ERROR: robot SN missing", file=sys.stderr)
return 2
import flexivrdk
print(f"=== RDK lite gripper SN={sn} device={device} iface={iface or 'auto'} ===")
robot = connect_lite(sn, iface)
dev = flexivrdk.Device(robot)
names = list(dev.list() or [])
if args.cmd == "list":
print("Studio devices:")
for n in names:
try:
en = dev.enabled(n)
cn = dev.connected(n)
except Exception:
en, cn = "?", "?"
mark = " <--" if n == device else ""
print(f" {n}: enabled={en} connected={cn}{mark}")
return 0
if device not in names:
print(
f"ERROR: device {device!r} not in Studio pool.\n available: {', '.join(names)}",
file=sys.stderr,
)
return 1
print(
f" Studio: enabled={dev.enabled(device)} connected={dev.connected(device)}"
)
if not dev.connected(device):
print(
"WARN: connected=False — 控制器自己的 /dev/ttyUSB0 没接到爪。\n"
" USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n"
" 或用法兰工具口 485（腕部白绿+/绿-）。"
)
grip = flexivrdk.Gripper(robot)
if args.cmd in ("enable", "status", "init", "open", "close", "pos"):
try:
grip.Enable(device)
print(f"Enable [{device}] OK")
except Exception as exc:
print(f"ERROR: Enable failed: {exc}", file=sys.stderr)
return 1
params = grip.params()
print(" params:", _fmt_params(params))
print(" states:", _fmt_states(grip.states()))
force = float(args.force_n or g.get("force_n") or 0.0)
if force <= 0:
force = 0.5 * (float(params.min_force) + float(params.max_force))
vel = float(args.vel or g.get("vel_mps") or 0.0)
if vel <= 0:
vel = 0.5 * float(params.max_vel)
if args.cmd in ("enable", "status"):
if not dev.connected(device):
return 2
return 0
if args.cmd == "init":
print("Init (fingers home, ~10 s) …")
grip.Init()
time.sleep(2.0)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "open":
_move(grip, params, float(params.max_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "close":
_move(grip, params, float(params.min_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "pos":
if args.value == "":
print("usage: pos <meters|0-1000>", file=sys.stderr)
return 2
raw = float(args.value)
if raw > 1.5:
span = float(params.max_width) - float(params.min_width)
width = float(params.min_width) + (raw / 1000.0) * span
else:
width = raw
_move(grip, params, width, vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
return 0
if __name__ == "__main__":
try:
raise SystemExit(main())
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
raise SystemExit(1)
scripts/rdk_switch_tool.py
Executable
+292
View File
@@ -0,0 +1,292 @@
#!/usr/bin/env python3
"""Switch the Flexiv active tool (TCP + payload) to a Studio-configured name.
Must run under RDK Python (rdk_py310). Robot must be IDLE; stop run_real.sh first
if ros2_control already owns the RDK connection.
/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 is single-arm (no JointGroup). Older builds use 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] # sentinel: call Tool.name()/Switch() without a 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 "arm"
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(
"robot not operational — AUTO Remote + Enable, release E-stop"
)
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(
"tool %r not found in Studio pool.\n available: %s"
% (want, ", ".join(names) or "(empty)")
)
def _iface_ipv4s(name: str) -> list:
"""FastDDS whitelist wants IPv4 addresses, not NIC names like 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
# unique, keep order
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]]:
"""RDK 手册要的是「连机器人的那张网卡」。enp4s0 上还有办公网 IP，不能整张卡的地址都塞进白名单。"""
import os
lists: list[list[str]] = []
pc = (os.environ.get("SITE_PC_IP") or "").strip()
if pc:
lists.append([pc])
if iface:
lists.append([iface])
robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
if robot_lan:
lists.append(robot_lan)
# unique
seen: set[tuple[str, ...]] = 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 connect_rdk(sn: str, iface: str = ""):
import flexivrdk
for listen in _rdk_whitelist(iface):
print("RDK whitelist:", ", ".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(" whitelist rejected, try next")
break
if "Failed to connect" in msg or "failed to connect" in msg.lower():
print(" search timeout with this whitelist")
break
raise
print("retry without iface filter")
return flexivrdk.Robot(sn)
def main() -> int:
ap = argparse.ArgumentParser(description="Switch Flexiv Studio tool / TCP")
ap.add_argument("--sn", default="", help="robot serial (default: site.yaml)")
ap.add_argument("--iface", default="", help="NIC whitelist for RDK (e.g. enp4s0)")
ap.add_argument(
"--name",
default="",
help="Studio tool name (Flange = no tool). Empty = site.yaml robot.tool_name",
)
ap.add_argument("--list", action="store_true", help="list tools and exit")
args = ap.parse_args()
sn = (args.sn or "").strip()
want = (args.name or "").strip()
if not sn:
print("ERROR: --sn is required (run via 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 tool SN={sn} iface={iface or 'auto'} ===")
robot = None
try:
robot = connect_rdk(sn, iface)
if robot.fault():
print("fault present, ClearFault ...")
if not robot.ClearFault():
print("ERROR: fault cannot be cleared", file=sys.stderr)
return 1
tool = flexivrdk.Tool(robot)
names = list(tool.list() or [])
groups = _single_arm_groups(robot)
print("Studio tools:", ", ".join(names) or "(none)")
for g in groups:
cur = _tool_name(tool, g)
extra = ""
try:
extra = " " + _fmt_params(_tool_params(tool, g))
except Exception:
pass
print(f" active [{_group_name(g)}]: {cur}{extra}")
if args.list:
return 0
if not want:
print("tool_name empty — keep current (set robot.tool_name in site.yaml to switch)")
print("RDK OK")
return 0
target = _resolve_name(tool, want)
already = all(_tool_name(tool, g) == target for g in groups)
if already:
print(f"already on [{target}]")
print("RDK OK")
return 0
print(f"switching → [{target}] (requires 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" now [{_group_name(g)}]: {cur}{extra}")
if cur != target:
print(f"ERROR: switch failed, still {cur!r}", file=sys.stderr)
return 1
print("RDK OK")
return 0
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(" Stop run_real.sh first (RDK is exclusive).", file=sys.stderr)
print(" Pendant: AUTO Remote + Enable Ethernet/RDK.", file=sys.stderr)
return 1
finally:
robot = None
if __name__ == "__main__":
raise SystemExit(main())
scripts/rdk_switch_tool.sh
Executable
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env bash
# Switch / list Flexiv Studio tools via RDK (reads name from site.yaml).
#
# bash ~/ros2/scripts/rdk_switch_tool.sh # switch to robot.tool_name
# bash ~/ros2/scripts/rdk_switch_tool.sh --list # print Studio tool pool
# bash ~/ros2/scripts/rdk_switch_tool.sh --name Flange
#
# Stop run_real.sh first — RDK allows only one client.
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
NAME="${SITE_ROBOT_TOOL_NAME:-}"
IFACE="${SITE_NET_IFACE:-}"
LIST=false
while [[ $# -gt 0 ]]; do
case "$1" in
--list|-l) LIST=true; shift ;;
--name) NAME="$2"; shift 2 ;;
--sn) SN="$2"; shift 2 ;;
--iface) IFACE="$2"; shift 2 ;;
-h|--help) sed -n '2,12p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
ARGS=(--sn "${SN}")
if [[ -n "${IFACE}" ]]; then
ARGS+=(--iface "${IFACE}")
fi
if [[ "${LIST}" == true ]]; then
ARGS+=(--list)
elif [[ -n "${NAME}" ]]; then
ARGS+=(--name "${NAME}")
fi
exec "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${ARGS[@]}"
scripts/run_camera_d405.sh
Executable
+158
View File
@@ -0,0 +1,158 @@
#!/usr/bin/env bash
# Terminal: D405 via official pyrealsense2 SDK — color + depth + stereo IR topics.
# With --preview the Qt window runs in a SEPARATE viewer process (realsense_view)
# so painting never slows the camera stream. Closing the window ≠ stop camera.
# Do NOT start view_realsense_depth.sh at the same time (USB is exclusive).
#
# Usage:
# 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 = use site; 0/1 = force
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"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
pkill -f 'lib/arm_application/realsense_color_pub' 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_NAME="${SITE_CAM_NAME:-d405}"
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 [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
CAM_ARGS+=(-p enable_infra:=true)
else
CAM_ARGS+=(-p enable_infra:=false)
fi
if [[ "${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 "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="
if [[ "${DO_PREVIEW}" == "1" ]]; then
# Camera publishes only; the Qt viewer runs in its own process so painting
# can never starve the capture loop (shared GIL cost ~70 ms/frame before).
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
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
scripts/run_camera_d435.sh
Executable
+50
View File
@@ -0,0 +1,50 @@
#!/usr/bin/env bash
# Overhead D435 between the two arms (eye-to-hand). Does not open either D405.
#
# 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"
# 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 -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
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
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:=d435
else
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi
scripts/run_display.sh
Executable
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
# View URDF only (joint_state_publisher_gui + RViz)
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
ros2 launch arm_description display.launch.py
scripts/run_dual_gripper.sh
Executable
+25
View File
@@ -0,0 +1,25 @@
#!/usr/bin/env bash
# Both DH grippers (left PGEA + right RGI) as one ROS 2 node.
# bash ~/ros2/scripts/run_dual_gripper.sh
# Topics: /dual_gripper_driver/left|right/{command,status}
# Services: /dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
sleep 0.3
echo "=== dual gripper PGEA ${SITE_GRIP_PORT:-/dev/ttyUSB0} + RGI (site.yaml robots[]) ==="
exec ros2 launch arm_application dual_gripper.launch.py sides:=left,right allow_offline:=true
scripts/run_gripper.sh
Executable
+131
View File
@@ -0,0 +1,131 @@
#!/usr/bin/env bash
# DH grippers on the IPC (Modbus RTU). Not Flexiv Studio / RDK.
#
# Left 89 PGEA parallel /dev/ttyUSB0
# Right 91 RGI rotary /dev/ttyUSB1
#
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli close
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli pos 500
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS node PGEA
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS node RGI
# bash ~/ros2/scripts/run_dual_gripper.sh # both ROS nodes
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CLI=()
DUAL=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
--cli) shift; CLI=("$@"); break ;;
--dual) DUAL=1; shift ;;
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
BACKEND="${SITE_GRIP_BACKEND:-usb}"
MODEL="${SITE_GRIP_MODEL:-PGEA-100-20-W}"
ARM="${SITE_ROBOT_ARM:-}"
IS_ROTARY=0
if [[ "${MODEL}" == *RGI* || "${ARM}" == "right" ]]; then
IS_ROTARY=1
fi
if [[ "${BACKEND}" == "rdk" || "${BACKEND}" == "flexiv" ]]; then
echo "ERROR: gripper.backend=${BACKEND} is Flexiv Studio. Use usb (IPC Modbus)." >&2
exit 1
fi
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
if [[ ${#CLI[@]} -gt 0 ]]; then
if [[ "${IS_ROTARY}" -eq 1 ]]; then
CMD="${CLI[0]}"
REST=("${CLI[@]:1}")
ARGS=(--arm right --test "${CMD}")
case "${CMD}" in
probe|status) ARGS=(--arm right --test probe) ;;
pos|move)
ARGS=(--arm right --test pos)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--pos "${REST[0]}"); fi
;;
rotate|rotate_to)
ARGS=(--arm right --test rotate)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
;;
revolutions)
ARGS=(--arm right --test rotate)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
;;
esac
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=91 ==="
exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
fi
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
BAUD="${SITE_GRIP_BAUD:-115200}"
SLAVE="${SITE_GRIP_SLAVE:-1}"
echo "=== DH ${MODEL} SN=${SITE_GRIP_SN:-?} ${PORT} @${BAUD} slave=${SLAVE} ==="
exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \
--port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \
--force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \
--rts "${SITE_GRIP_RS485_RTS:-}" \
"${CLI[@]}"
fi
if [[ "${DUAL}" -eq 1 ]]; then
exec bash "${ROOT}/scripts/run_dual_gripper.sh"
fi
if [[ "${IS_ROTARY}" -eq 1 ]]; then
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
sleep 0.3
echo "=== DH RGI ROS node ${SITE_GRIP_PORT:-/dev/ttyUSB1} (arm_msgs dual_gripper right) ==="
exec ros2 launch arm_application dual_gripper.launch.py sides:=right allow_offline:=true
fi
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
BAUD="${SITE_GRIP_BAUD:-115200}"
SLAVE="${SITE_GRIP_SLAVE:-1}"
GSN="${SITE_GRIP_SN:-}"
PY="${ROOT}/src/arm_application/arm_application/dh_pgea.py"
NODE_PY="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
if [[ ! -e "${PORT}" ]]; then
echo "ERROR: ${PORT} missing. Plug USB–RS485 into the IPC." >&2
exit 1
fi
pkill -f 'arm_application/dh_pgea_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dh_pgea' 2>/dev/null || true
sleep 0.4
echo "=== DH ${MODEL} SN=${GSN:-?} ${PORT} @${BAUD} ROS node ==="
exec python3 "${NODE_PY}" --ros-args \
-p port:="${PORT}" \
-p baud:="${BAUD}" \
-p slave_id:="${SLAVE}" \
-p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \
-p force_pct:="${SITE_GRIP_FORCE:-50}" \
-p speed_pct:="${SITE_GRIP_SPEED:-50}" \
-p auto_init:="${SITE_GRIP_AUTO_INIT:-true}" \
-p model:="${MODEL}" \
-p "serial_number:='${GSN}'" \
-p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'"
scripts/run_handeye.sh
Executable
+254
View File
@@ -0,0 +1,254 @@
#!/usr/bin/env bash
# Hand-eye calibration helpers.
# Scripts: ~/ros2/scripts/run_handeye.sh
# Package YAML: ~/ros2/src/arm_application/config/handeye_*.yaml
# Site overrides (board / motion): ~/ros2/config/site.yaml → handeye:
# Runtime output: /tmp/handeye_calib/
#
# Usage:
# bash ~/ros2/scripts/run_handeye.sh calibrate
# bash ~/ros2/scripts/run_handeye.sh waypoints
# bash ~/ros2/scripts/run_handeye.sh teach # RDK 软拖拽（推荐）
# bash ~/ros2/scripts/run_handeye.sh teach --append # 接着上次点位继续采
# bash ~/ros2/scripts/run_handeye.sh teach --ros # 示教器 Free Drive
# bash ~/ros2/scripts/run_handeye.sh merge [taught.txt]
# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
# bash ~/ros2/scripts/run_handeye.sh --arm 91 calibrate
#
# calibrate / waypoints 需要:
# T1: bash ~/ros2/scripts/run_real.sh --arm 89|91
# T2: bash ~/ros2/scripts/run_camera_d405.sh --arm 89|91 --preview
# teach（拖拽）需要: 先停掉 run_real.sh 和相机（RDK + D405 都独占）
# 89 与 91 的结果文件、示教点、workdir 完全分开，禁止混用。
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
CMD="${1:-}"
shift || true
usage() {
sed -n '2,18p' "$0"
}
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# Prefer package source tree (editable); fall back to install share
APP_CFG="${ROOT}/src/arm_application/config"
if [[ ! -d "${APP_CFG}" ]]; then
APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
fi
WP="${SITE_HANDEYE_WAYPOINTS:-${APP_CFG}/handeye_poses.yaml}"
CAL="${SITE_HANDEYE_CALIBRATE:-${APP_CFG}/handeye_calibrate.yaml}"
TOUR="${SITE_HANDEYE_TOUR:-${APP_CFG}/handeye_waypoints.yaml}"
RES="${SITE_HANDEYE_FILE:-${APP_CFG}/handeye_result.yaml}"
WORKDIR="${SITE_HANDEYE_WORKDIR:-/tmp/handeye_calib}"
TAUGHT="${WORKDIR}/taught_poses.txt"
PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}"
CAM="${SITE_CAM_NAME:-d405}"
_kill_servo() {
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
sleep 1
}
_need_files() {
local f
for f in "$@"; do
if [[ ! -f "$f" ]]; then
echo "ERROR: missing $f" >&2
exit 2
fi
done
}
_need_robot() {
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: start run_real.sh first" >&2
exit 1
fi
}
_need_camera() {
if ! ros2 topic list 2>/dev/null | grep -q "/${CAM}/color/image_raw"; then
echo "ERROR: no /${CAM}/color/image_raw — start run_camera_d405.sh" >&2
exit 1
fi
}
cmd_show() {
echo "=== hand-eye (package + site) ==="
echo " arm : ${SITE_ROBOT_SN:-} ${SITE_ROBOT_LABEL:-}"
echo " package cfg : ${APP_CFG}"
echo " calibrate : ${CAL}"
echo " poses : ${WP}"
echo " tour : ${TOUR}"
echo " result : ${RES}"
echo " workdir : ${WORKDIR}"
echo " board : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
if [[ -f "${WP}" ]]; then
python3 - <<PY
import re
from pathlib import Path
t = Path("${WP}").read_text(encoding="utf-8")
m = re.search(r"num_poses:\s*(\d+)", t)
n = int(m.group(1)) if m else "?"
print(f" num_poses in poses yaml: {n}")
PY
fi
}
cmd_merge() {
local src="${1:-${TAUGHT}}"
_need_files "${src}" "${WP}"
python3 "${ROOT}/scripts/merge_handeye_poses.py" "${src}" "${WP}"
echo "=== next: bash ~/ros2/scripts/run_handeye.sh calibrate ==="
}
cmd_calibrate() {
_need_files "${CAL}" "${WP}"
_need_robot
_need_camera
echo "=== kill visual_servo (avoid traj fight) ==="
_kill_servo
mkdir -p "${WORKDIR}"
local cols="${SITE_HANDEYE_BOARD_COLS:-11}"
local rows="${SITE_HANDEYE_BOARD_ROWS:-16}"
local sq="${SITE_HANDEYE_SQUARE_M:-0.011}"
local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
local qd="${SITE_HANDEYE_SETTLE_QD:-0.015}"
local ss="${SITE_HANDEYE_SETTLE_STABLE:-0.6}"
local sto="${SITE_HANDEYE_SETTLE_TO:-8.0}"
local ph="${SITE_HANDEYE_PIN_HOLDS:-2}"
local ps="${SITE_HANDEYE_PIN_SEC:-0.45}"
local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}"
echo "=== calibrate mode=${SITE_HANDEYE_MODE:-eye_in_hand} ==="
echo " board ${cols}x${rows} inner, square=${sq} m (from site.yaml)"
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
echo " poses=${WP}"
echo " result → ${RES}"
exec ros2 run arm_application handeye_calibrate --ros-args \
--params-file "${CAL}" \
--params-file "${WP}" \
-p joint_prefix:="${PREFIX}" \
-p camera_source:="${CAM}" \
-p output_dir:="${WORKDIR}" \
-p result_file:="${RES}" \
-p mode:="${SITE_HANDEYE_MODE:-eye_in_hand}" \
-p board_cols:="${cols}" \
-p board_rows:="${rows}" \
-p square_size_m:="${sq}" \
-p velocity_scale:="${vel}" \
-p move_time_sec:="${mt}" \
-p settle_sec:="${st}" \
-p settle_qd_max:="${qd}" \
-p settle_stable_sec:="${ss}" \
-p settle_timeout_sec:="${sto}" \
-p pin_holds:="${ph}" \
-p pin_hold_sec:="${ps}" \
-p skip_unstable_poses:="${skip}"
}
cmd_waypoints() {
_need_files "${TOUR}" "${WP}"
_need_robot
_need_camera
echo "=== kill visual_servo ==="
_kill_servo
mkdir -p "${WORKDIR}"
echo "=== waypoint tour (Enter=confirm, q=quit) ==="
echo " saves confirmed joints → ${TAUGHT}"
echo " then: bash ~/ros2/scripts/run_handeye.sh merge"
exec ros2 run arm_application handeye_waypoints --ros-args \
--params-file "${TOUR}" \
--params-file "${WP}" \
-p joint_prefix:="${PREFIX}" \
-p camera_source:="${CAM}" \
-p append_poses_file:="${TAUGHT}"
}
cmd_teach() {
local backend="rdk"
local append=false
while [[ $# -gt 0 ]]; do
case "$1" in
--rdk) backend="rdk"; shift ;;
--ros|--pendant) backend="ros"; shift ;;
--append|--keep) append=true; shift ;;
*) echo "Unknown teach arg: $1" >&2; exit 1 ;;
esac
done
echo "=== kill visual_servo + camera (D405 exclusive) ==="
_kill_servo
pkill -f 'run_camera_d405' 2>/dev/null || true
pkill -f 'realsense_color_pub' 2>/dev/null || true
pkill -f 'realsense2_camera_node' 2>/dev/null || true
sleep 1
if [[ "${backend}" == "rdk" ]]; then
if ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: RDK 拖拽与 run_real.sh 互斥。请先在那个终端 Ctrl-C 停掉 run_real.sh" >&2
exit 1
fi
else
_need_robot
fi
mkdir -p "${WORKDIR}/images"
if [[ "${append}" == true ]]; then
echo "=== append: keep existing ${TAUGHT} ==="
else
# 重新设点：丢掉上次 taught，避免 merge 混进旧位姿
: > "${TAUGHT}"
fi
echo "=== interactive teach backend=${backend} → ${WORKDIR} ==="
echo " 窗口按键: d=拖拽开 s=拖拽关 Enter=拍照记点 q=结束"
echo " 建议 8–12 个姿态，棋盘占画面约 30–70%"
echo " 结束后: bash ~/ros2/scripts/run_handeye.sh merge"
echo " bash ~/ros2/scripts/run_handeye.sh calibrate"
TEACH_ARGS=(
-p backend:="${backend}"
-p robot_sn:="${SITE_ROBOT_SN:-Rizon4-063989}"
-p joint_prefix:="${PREFIX}"
-p camera_source:="${CAM}"
-p output_dir:="${WORKDIR}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
fi
if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then
TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}")
fi
TEACH_ARGS+=(
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}"
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}"
-p gain:="${SITE_CAM_GAIN:-0}"
)
exec ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}"
}
case "${CMD}" in
calibrate|calib) cmd_calibrate ;;
waypoints|tour) cmd_waypoints "$@" ;;
teach) cmd_teach "$@" ;;
merge) cmd_merge "$@" ;;
show) cmd_show ;;
-h|--help|help|"") usage; [[ -n "${CMD}" ]] || exit 0; exit 0 ;;
*) echo "Unknown: ${CMD}" >&2; usage >&2; exit 1 ;;
esac
scripts/run_hole_detect.sh
Executable
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env bash
# Hole detection on the current-arm wrist D405. Detect only — no insert motion.
#
# bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
#
# backend from site.yaml hole_detect.backend:
# two_stage YOLO workpiece bbox + HoughCircles (camera_test hole_detect.py)
# yolo YOLO boxes treated as holes
# hough full-image HoughCircles
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=false
# 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=true; shift ;;
-h|--help) sed -n '2,16p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
pkill -f 'arm_application/hole_detect_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/hole_detect' 2>/dev/null || true
sleep 0.2
BACKEND="${SITE_HOLE_BACKEND:-two_stage}"
echo "=== hole_detect arm=${SITE_ROBOT_ARM:-?} cam=${SITE_CAM_NAME} backend=${BACKEND} preview=${PREVIEW} ==="
exec ros2 launch arm_application hole_detect.launch.py \
backend:="${BACKEND}" \
preview:="${PREVIEW}"
scripts/run_mock_demo.sh
Executable
+11
View File
@@ -0,0 +1,11 @@
#!/usr/bin/env bash
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
source /home/cc/anaconda3/etc/profile.d/conda.sh
conda activate ros_flexiv
source "${ROOT}/install/setup.bash"
ros2 run arm_application demo_named_poses --ros-args \
-p "poses:=['home','ready','inspect','pos_left','home']" \
-p velocity_scale:=2.0 \
-p dwell_sec:=0.2 \
-p loops:=1
scripts/run_pipeline.sh
Executable
+3
View File
@@ -0,0 +1,3 @@
#!/usr/bin/env bash
# 三阶段串联的别名，等价于 run_all.sh。
exec "$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/run_all.sh" "$@"
scripts/run_real.sh
Executable
+96
View File
@@ -0,0 +1,96 @@
#!/usr/bin/env bash
# Real Rizon4 bringup — sets RDK runtime libs + launches stack backend:=real
# Note: no `set -u` — conda/RoboStack hooks reference unset vars (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
while [[ $# -gt 0 ]]; do
case "$1" in
--sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
--list|-l) LIST_ONLY=true; shift ;;
-h|--help)
sed -n '2,8p' "$0"
echo " Extra args are passed to ros2 launch (e.g. use_rviz:=false)."
exit 0
;;
*) break ;;
esac
done
source /home/cc/anaconda3/etc/profile.d/conda.sh
conda activate ros_flexiv
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# flexiv_hardware must load spdlog/RBDyn from flexiv_install (not conda fmt v11)
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
# SITE_ROBOT_SN is the resolved Flexiv serial (89/91 aliases already mapped).
ROBOT_SN="${SITE_ROBOT_SN:-${ROBOT_SN:-Rizon4-063989}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
NET_IFACE="${SITE_NET_IFACE:-}"
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/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:-} ==="
echo "=== stop leftover ros2_control (RDK exclusive) ==="
pkill -f '/controller_manager/ros2_control_node' 2>/dev/null || true
pkill -f 'arm_bringup.*stack.launch' 2>/dev/null || true
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}" "$@"
scripts/run_sim.sh
Executable
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
# Quick-start: mock arm + RViz
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
ros2 launch arm_bringup sim.launch.py
scripts/run_stage2_locate.sh
Executable
+136
View File
@@ -0,0 +1,136 @@
#!/usr/bin/env bash
# 二阶段孔定位（一次性拍照，不做实时推理）：垂直朝下 → 悬停锁高 → 拍照 →
# 解 k1/k2 孔基座坐标 → 夹爪/通止规轴线对准目标孔 → 锁定保持。
#
# 前置（各占一个终端）：
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器（二阶段先只做左臂）
# T3 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了传感器）。
#
# 用法：
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整定位+对准 k1
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬停锁定（调高度）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高（不改 yaml）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --no-tactile # 不起触觉服务
#
# 悬停锁高固定接口：config/site.yaml → robots[89].servo.hole_hover_z_m
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
HOVER_ONLY=false
HOLE=""
HOVER_Z=""
WITH_TACTILE=true
# 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
--hover-only) HOVER_ONLY=true; shift ;;
--hole) HOLE="${2:?}"; shift 2 ;;
--hover-z) HOVER_Z="${2:?}"; shift 2 ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,22p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# 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 "=== stage2 locate arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
# ---- 1) 机械臂控制器必须在（和 run_visual_servo_rt.sh 同一套判活） ----
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
ready=false
for _i in $(seq 1 30); do
if timeout 5 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
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活。先跑: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
exit 1
fi
# ---- 2) 相机话题必须在发（二阶段拍照用；--hover-only 纯调高度，不需要相机） ----
CAM_NS="${SITE_CAM_NAME:-d405_left}"
IMG_TOPIC="/${CAM_NS}/color/image_raw"
if [[ "${HOVER_ONLY}" == true ]]; then
echo "=== hover-only：跳过相机/触觉检查，直接垂直+悬停 ==="
else
echo "=== check camera topic ${IMG_TOPIC} ==="
cam_ok=false
for _i in $(seq 1 24); do # 最多等 ~12s，相机可能正在起
# 以真实消息为准（topic info 的 Publisher count 依赖 DDS 发现，进程频繁重启后会假阴性）
if timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"; then
cam_ok=true
break
fi
sleep 0.5
done
if [[ "${cam_ok}" != true ]]; then
echo "ERROR: ${IMG_TOPIC} 没有发布者。先开相机：" >&2
echo " bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview" >&2
exit 1
fi
echo "camera ${CAM_NS} publishing OK"
fi
# ---- 3) 触觉传感器服务（目前只有左臂 89 装了传感器；hover-only 不需要） ----
if [[ "${WITH_TACTILE}" == true && "${HOVER_ONLY}" != true ]]; then
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
echo "=== start tactile sensor service (left arm) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
sleep 1.0
nohup ros2 launch arm_application tactile.launch.py > /tmp/stage2_tactile.log 2>&1 &
echo " tactile pid=$! log=/tmp/stage2_tactile.log"
t0=$SECONDS
while (( SECONDS - t0 < 30 )); do
if timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true"; then
echo " /tactile/left/status publishing OK"
break
fi
sleep 0.5
done
if ! timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true"; then
echo "WARN: 触觉话题还没起来（传感器没接？），定位流程继续。日志: /tmp/stage2_tactile.log" >&2
fi
else
echo "=== 触觉传感器只有左臂有；当前 ${SITE_ROBOT_ARM}，跳过（--no-tactile 可关此提示） ==="
fi
fi
# ---- 4) 清掉可能打架的旧进程（一阶段伺服/旧二阶段） ----
echo "=== stop old perception/servo/stage2 ==="
pkill -f 'lib/arm_application/stage2_locate' 2>/dev/null || true
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
sleep 1
echo "=== 确认：左臂夹爪已夹持通止规，通止规轴线与法兰轴线一致 ==="
# ---- 5) 起二阶段定位 ----
ARGS=(hover_only:="${HOVER_ONLY}")
if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
if [[ -n "${HOVER_Z}" ]]; then ARGS+=(hover_z_m:="${HOVER_Z}"); fi
echo "=== launch stage2_locate hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} ==="
exec ros2 launch arm_application stage2_locate.launch.py "${ARGS[@]}"
scripts/run_stage3_search.sh
Executable
+152
View File
@@ -0,0 +1,152 @@
#!/usr/bin/env bash
# 三阶段柔顺搜孔：触面 → 力闭环螺旋搜孔 → 十字定心 → 力限插入/旋入 → 锁定。
# 前置（各占一个终端）：
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器
# —— 二阶段已完成（run_stage2_locate.sh --arm 89），臂保持悬停不动
# 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了 Orisys 传感器）。
#
# 用法：
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座（site 默认深度）
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 # 对 k2 孔
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入(M6=1.0)
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile # 不起触觉服务（调试用）
#
# 参数固定接口：config/site.yaml → hole_detect.stage3（力阈值/螺旋/插入全部可调）
# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看 Orisys 读数量级
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
HOLE=""
DEPTH=""
PITCH=""
RMAX=""
RESNAP=false
WITH_TACTILE=true
# 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
--hole) HOLE="${2:?}"; shift 2 ;;
--depth) DEPTH="${2:?}"; shift 2 ;;
--pitch) PITCH="${2:?}"; shift 2 ;;
--r-max) RMAX="${2:?}"; shift 2 ;;
--resnap) RESNAP=true; shift ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# 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 "=== stage3 search arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
# ---- 1) 机械臂控制器必须在 ----
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
ready=false
for _i in $(seq 1 30); do
if timeout 5 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
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活。先跑: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
exit 1
fi
# ---- 2) 触觉传感器服务（三阶段的核心传感器，必须在线） ----
if [[ "${WITH_TACTILE}" == true ]]; then
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
echo "=== start tactile sensor service (left arm) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
sleep 1.0
nohup ros2 launch arm_application tactile.launch.py > /tmp/stage3_tactile.log 2>&1 &
echo " tactile pid=$! log=/tmp/stage3_tactile.log"
tac_ok=false
for _i in $(seq 1 15); do
# 必须看到真实消息（Publisher count 只反映 DDS 端点存在，不代表数据在流）
# 单侧可用即可（另一指掉线时降级为单指模式，stage3 节点内会告警）
l_live=false; r_live=false
timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true" && l_live=true
timeout 4 ros2 topic echo /tactile/right/status --once 2>/dev/null | grep -q "connected: true" && r_live=true
if [[ "${l_live}" == true || "${r_live}" == true ]]; then
tac_ok=true
[[ "${l_live}" == true && "${r_live}" == true ]] || \
echo " WARN: 单指触觉（左=${l_live} 右=${r_live}）——另一指掉线，降级运行"
break
fi
sleep 0.5
done
if [[ "${tac_ok}" != true ]]; then
echo "ERROR: /tactile/left|right/status 双侧都没起来——三阶段必须有触觉。日志: /tmp/stage3_tactile.log" >&2
echo " 排查: bash ~/ros2/scripts/run_tactile.sh --cli；若报 Failed to open video source: pkill orisys_tac_w 后重试" >&2
exit 1
fi
echo " /tactile status publishing OK"
else
echo "ERROR: 触觉传感器只有左臂 89 有；当前 ${SITE_ROBOT_ARM} 不能跑三阶段" >&2
exit 1
fi
fi
# ---- 3) 二阶段结果参考（不强制，只提示） ----
if [[ -f /tmp/stage2_result.json ]]; then
age=$(( $(date +%s) - $(stat -c %Y /tmp/stage2_result.json) ))
echo "=== stage2 result: $(date -d @$(stat -c %Y /tmp/stage2_result.json) '+%H:%M:%S') (${age}s 前) ==="
if (( age > 900 )); then
echo "WARN: 二阶段结果已超过 15 分钟——工件若被动过请重跑二阶段" >&2
fi
else
echo "WARN: 没有 /tmp/stage2_result.json——确认二阶段已跑过且臂仍悬停在孔上方" >&2
fi
# ---- 4) 复拍模式需要相机 ----
if [[ "${RESNAP}" == true ]]; then
CAM_NS="${SITE_CAM_NAME:-d405_left}"
echo "=== resnap：check camera topic /${CAM_NS}/color/image_raw ==="
if ! timeout 5 ros2 topic info "/${CAM_NS}/color/image_raw" 2>/dev/null | grep -q "Publisher count: [1-9]"; then
echo "ERROR: 相机没开。先: bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview" >&2
exit 1
fi
fi
# ---- 5) 清掉可能打架的旧进程 ----
echo "=== stop old perception/servo/stage2/stage3 ==="
pkill -f 'lib/arm_application/stage3_search' 2>/dev/null || true
pkill -f 'lib/arm_application/stage2_locate' 2>/dev/null || true
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
sleep 1
echo "=== 确认：臂仍悬停在目标孔上方（二阶段完成后没动过），通止规夹持牢固 ==="
# ---- 6) 起三阶段搜孔 ----
ARGS=(resnap:="${RESNAP}")
if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
if [[ -n "${DEPTH}" ]]; then ARGS+=(insert_depth_mm:="${DEPTH}"); fi
if [[ -n "${PITCH}" ]]; then ARGS+=(screw_pitch_mm:="${PITCH}"); fi
if [[ -n "${RMAX}" ]]; then ARGS+=(spiral_r_max_mm:="${RMAX}"); fi
echo "=== launch stage3_search hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} r_max=${RMAX:-site} resnap=${RESNAP} ==="
exec ros2 launch arm_application stage3_search.launch.py "${ARGS[@]}"
scripts/run_tactile.sh
Executable
+34
View File
@@ -0,0 +1,34 @@
#!/usr/bin/env bash
# Orisys visuo-tactile (finger cameras, not D405). Config: site.yaml tactile:
#
# bash ~/ros2/scripts/run_tactile.sh --cli
# bash ~/ros2/scripts/run_tactile.sh # ROS node /tactile/left|right/status
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CLI=0
while [[ $# -gt 0 ]]; do
case "$1" in
--cli) CLI=1; shift ;;
-h|--help) sed -n '2,10p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
echo "=== tactile backend=${SITE_TACTILE_BACKEND:-orisys} L=${SITE_TACTILE_LEFT:-0} R=${SITE_TACTILE_RIGHT:-2} cfg=${SITE_TACTILE_CONFIG:-dd03} ==="
if [[ "${CLI}" -eq 1 ]]; then
exec python3 -m arm_application.tactile_cli
fi
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
sleep 0.2
exec ros2 launch arm_application tactile.launch.py
scripts/run_visual_servo_rt.sh
Executable
+83
View File
@@ -0,0 +1,83 @@
#!/usr/bin/env bash
# Real-time visual servo (reads ALL tunables from ~/ros2/config/site.yaml).
#
# bash ~/ros2/scripts/run_visual_servo_rt.sh
# bash ~/ros2/scripts/run_visual_servo_rt.sh --control
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
#
# --control starts tracking from the current pose (no lift, no return-to-home).
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# Switch YOLO weights: edit paths.yolo_weights in site.yaml
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CONTROL=false
# 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
--control) CONTROL=true; shift ;;
-h|--help) sed -n '2,12p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
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}"
echo "=== FastDDS: ${FASTRTPS_DEFAULT_PROFILES_FILE} ==="
fi
if [[ "${CONTROL}" == true ]]; then
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
# NOTE: action-list can show zombie entries from a crashed ros2_control,
# and `ros2 control` CLI is broken in this env (xmlrpc Fault). A raw
# service call only answers when the controller manager is actually alive.
ready=false
for _i in $(seq 1 40); do
if timeout 5 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
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活（可能 T1 已崩溃或机器人不在网）。" >&2
echo " 先确认 ping 通机器人，再重启 T1 并等 operational：" >&2
echo " bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-91}" >&2
exit 1
fi
echo "=== stop old perception/servo ==="
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
sleep 1
echo "=== start perception + servo CONTROL ON (lock_z from robots[].servo.ee_z_m) ==="
else
echo "=== launch perception + servo (observe only, no arm motion) ==="
fi
EE="${SITE_EE_FRAME:-Rizon4-063989_flange}"
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}"
LAUNCH_ARGS=(
enable_control:="${CONTROL}"
handeye_file:="${HANDEYE}"
ee_frame:="${EE}"
camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}"
)
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"
scripts/show_site_config.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/usr/bin/env bash
# Show / validate the master site config.
# bash ~/ros2/scripts/show_site_config.sh
# SITE_CONFIG=/path/to/other.yaml bash ~/ros2/scripts/show_site_config.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print ${ROBOT_SN:+--sn "$ROBOT_SN"}
echo
echo "Edit: ${SITE_CONFIG:-$ROOT/config/site.yaml}"
echo "Switch weights example:"
echo " paths.yolo_weights: \"/home/cc/ros2/models/yolo/detect_hc90.pt\""
echo "Or symlink:"
echo " ln -sfn detect_hc90.pt ~/ros2/models/yolo/active.pt"
echo " # then set paths.yolo_weights to .../active.pt"
scripts/test_rdk_python.sh
Executable
+43
View File
@@ -0,0 +1,43 @@
#!/usr/bin/env bash
# Smoke-test Flexiv RDK Python against site robot (no ROS / no GitHub).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
SN="${1:-${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
IFACE="${SITE_NET_IFACE:-}"
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
echo "Using: ${PY}"
echo "Robot SN: ${SN} IP: ${ROBOT_IP} iface: ${IFACE:-auto}"
echo "Ping ${ROBOT_IP} ..."
ping -c 1 -W 1 "${ROBOT_IP}" >/dev/null && echo " network OK" || echo " network FAIL"
"${PY}" - <<PY
import flexivrdk
sn = "${SN}"
iface = "${IFACE}"
print("Connecting...", sn, "iface", iface or "auto")
robot = None
if iface:
for args in ((sn, [iface]), (sn, [iface], True), (sn, [iface], True, False)):
try:
robot = flexivrdk.Robot(*args)
break
except TypeError:
continue
if robot is None:
robot = flexivrdk.Robot(sn)
print("Connected. Clearing fault if any...")
try:
robot.ClearFault()
except Exception as e:
print("ClearFault:", e)
mode = robot.mode()
print("mode:", mode)
states = robot.states()
print("q:", list(states.q))
print("OK — RDK Python link works. Enable RDK on teach pendant if mode/fault unexpected.")
PY
scripts/use_local_flexiv.sh
Executable
+50
View File
@@ -0,0 +1,50 @@
#!/usr/bin/env bash
# Wire local Flexiv trees under ~/文档/Flexiv_Rizon4 into this workspace.
# Avoids re-cloning RDK / sim plugin from GitHub.
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
TP="${ROOT}/thirdparty"
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ ! -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
echo "ERROR: not found: ${LOCAL_FLEXIV}/flexiv_rdk"
echo "Set LOCAL_FLEXIV to your Flexiv_Rizon4 directory."
exit 1
fi
mkdir -p "${SRC}" "${TP}"
VER_LINE=$(grep -E '^project\(flexiv_rdk' "${LOCAL_FLEXIV}/flexiv_rdk/CMakeLists.txt" | head -1 || true)
echo "Linking RDK (${VER_LINE:-unknown version})..."
ln -sfn "${LOCAL_FLEXIV}/flexiv_rdk" "${SRC}/flexiv_rdk"
touch "${LOCAL_FLEXIV}/flexiv_rdk/COLCON_IGNORE"
if [[ -d "${LOCAL_FLEXIV}/flexiv_sim_plugin" ]]; then
if [[ -d "${TP}/flexiv_sim_plugin" && ! -L "${TP}/flexiv_sim_plugin" ]]; then
mv "${TP}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin.cloned.bak.$(date +%s)"
fi
ln -sfn "${LOCAL_FLEXIV}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin"
touch "${LOCAL_FLEXIV}/flexiv_sim_plugin/COLCON_IGNORE"
echo "Linked thirdparty/flexiv_sim_plugin -> ${LOCAL_FLEXIV}/flexiv_sim_plugin"
fi
# humble-v1 ROS hardware needs RDK 1.9.x (kept separately if present)
if [[ -d "${SRC}/flexiv_rdk.bak.1787646725" ]]; then
ln -sfn "${SRC}/flexiv_rdk.bak.1787646725" "${SRC}/flexiv_rdk_v1"
touch "${SRC}/flexiv_rdk.bak.1787646725/COLCON_IGNORE"
echo "ROS stack RDK v1.9.1 available at src/flexiv_rdk_v1"
fi
cat <<EOF
Done.
src/flexiv_rdk -> ${LOCAL_FLEXIV}/flexiv_rdk (your tree, currently v2.1)
thirdparty/flexiv_sim_plugin -> local sim plugin
Note: flexiv_ros2 humble-v1 expects RDK **v1.9.1**.
Build/install src/flexiv_rdk_v1 into ~/flexiv_install for flexiv_hardware.
Use your v2.1 tree for RDK C++/Python examples and Isaac-side tooling.
EOF