arm_dev
7d22f777c8
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
113 lines
4.7 KiB
Bash
Executable File
113 lines
4.7 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# 实时视觉伺服（所有可调参数全部从 ~/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 从当前位姿直接开始跟踪（不抬升，也不回 home）。
|
||
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
|
||
#
|
||
# 跟踪相机由 site.yaml 的 perception.track_camera 决定：
|
||
# wrist 腕部 D405 主跟踪；画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0
|
||
# —— 需要同时开 run_camera_d405.sh 与 run_camera_d435.sh
|
||
# overhead 整段只用顶置 D435（实验）—— 另开 run_camera_d435.sh
|
||
#
|
||
# 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights
|
||
|
||
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:-?}) ==="
|
||
# 注意：action-list 可能显示出崩溃过的 ros2_control 留下的僵尸条目，
|
||
# 而且这个环境里的 `ros2 control` CLI 是坏的（xmlrpc Fault）。直接发
|
||
# 服务请求只有控制器管理器真的活着时才会应答。
|
||
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:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}"
|
||
TRACK="${SITE_TRACK_CAMERA:-wrist}"
|
||
HANDEYE="${SITE_TRACK_HANDEYE_FILE:-${SITE_HANDEYE_FILE:-}}"
|
||
CAM_FRAME="${SITE_TRACK_CAMERA_FRAME:-${SITE_CAMERA_FRAME:-}}"
|
||
CAM_NAME="${SITE_TRACK_CAM_NAME:-${SITE_CAM_NAME:-}}"
|
||
if [[ -z "${HANDEYE}" ]]; then
|
||
echo "ERROR: 跟踪用手眼文件为空（SITE_TRACK_HANDEYE_FILE / SITE_HANDEYE_FILE）" >&2
|
||
exit 2
|
||
fi
|
||
if [[ ! -f "${HANDEYE}" ]]; then
|
||
echo "ERROR: 跟踪用手眼文件不存在：${HANDEYE}" >&2
|
||
if [[ "${TRACK}" == "overhead" ]]; then
|
||
echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate --mode eye_to_hand" >&2
|
||
else
|
||
echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate" >&2
|
||
fi
|
||
exit 2
|
||
fi
|
||
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} track=${TRACK} camera=${CAM_NAME}"
|
||
echo " handeye=${HANDEYE} frame=${CAM_FRAME} control=${CONTROL}"
|
||
if [[ "${TRACK}" == "overhead" ]]; then
|
||
echo "=== 整段顶视：D435 追踪，D405 不参与一阶段 ==="
|
||
echo " 相机请开：bash ~/ros2/scripts/run_camera_d435.sh --preview"
|
||
else
|
||
echo "=== 本脚本不弹预览窗。工件框画在 D405 那个查看器上 ==="
|
||
echo " bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview"
|
||
echo " bash ~/ros2/scripts/run_camera_d435.sh --preview # 顶视可开，绿框仍在 D405 窗"
|
||
fi
|
||
LAUNCH_ARGS=(
|
||
enable_control:="${CONTROL}"
|
||
handeye_file:="${HANDEYE}"
|
||
ee_frame:="${EE}"
|
||
camera_frame:="${CAM_FRAME}"
|
||
)
|
||
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"
|