arm_dev
7942014763
验收：test_listed_arms 与 test_camera_preview 共 20 项通过。D435 1280×720 发布约 29 帧/秒。 Co-authored-by: Cursor <cursoragent@cursor.com>
245 lines
8.7 KiB
Bash
Executable File
245 lines
8.7 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# 工控机上的 DH 夹爪（Modbus RTU）。不是 Flexiv Studio / RDK 那一套。
|
||
#
|
||
# 左臂 89 PGEA 平行夹爪 /dev/ttyUSB0
|
||
# 右臂 91 RGI 旋转夹爪 /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 --force 40 --speed 20 --cli rotate_by -15
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
|
||
# --force / --speed 写在 --cli 前面，覆盖 site.yaml 里的力度和速度。
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS 节点 PGEA
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS 节点 RGI
|
||
# bash ~/ros2/scripts/run_dual_gripper.sh # 两个 ROS 节点都起
|
||
|
||
set -eo pipefail
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
CLI=()
|
||
DUAL=0
|
||
GRIP_FORCE=""
|
||
GRIP_SPEED=""
|
||
|
||
# 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
|
||
--force) GRIP_FORCE="${2:?}"; shift 2 ;;
|
||
--speed) GRIP_SPEED="${2:?}"; shift 2 ;;
|
||
--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"
|
||
|
||
# 平行爪节点起来之后串口是它的。再开一次会报占用，开合就失效。
|
||
# 这时把命令交给节点已经在听的 /gripper/ctrl。
|
||
pgea_node_up() {
|
||
local line
|
||
while IFS= read -r line; do
|
||
[[ "${line}" == *pgrep* ]] && continue
|
||
[[ -n "${line}" ]] && return 0
|
||
done < <(pgrep -af 'dh_pgea_node.py|lib/arm_application/dh_pgea' 2>/dev/null || true)
|
||
return 1
|
||
}
|
||
|
||
# 控制前把平行爪驱动拉起来。不 pkill 已经在跑的节点，也不写 0xA5。
|
||
# auto_init 用 site 里这一臂的值（现场是 false，回零会把夹着的件丢掉）。
|
||
# setsid：命令脚本退出后驱动还在，不会跟着会话一起被挂断。
|
||
ensure_pgea_node() {
|
||
if pgea_node_up; then
|
||
return 0
|
||
fi
|
||
local port="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
|
||
if [[ ! -e "${port}" ]]; then
|
||
echo "ERROR: ${port} 不在。USB–RS485 没插上，夹爪驱动起不来。" >&2
|
||
return 1
|
||
fi
|
||
local node_py="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
|
||
local short="${SITE_ROBOT_SHORT_ID:-89}"
|
||
local log_dir="/tmp/orisys_console"
|
||
local log="${log_dir}/gripper_node_${short}.log"
|
||
local auto="${SITE_GRIP_AUTO_INIT:-true}"
|
||
mkdir -p "${log_dir}"
|
||
echo "=== 平行爪驱动没在跑，先拉起来（auto_init=${auto}，不另写回零） ${port} ==="
|
||
setsid python3 "${node_py}" --ros-args \
|
||
-p port:="${port}" \
|
||
-p baud:="${SITE_GRIP_BAUD:-115200}" \
|
||
-p slave_id:="${SITE_GRIP_SLAVE:-1}" \
|
||
-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:="${auto}" \
|
||
-p model:="${MODEL}" \
|
||
-p "serial_number:='${SITE_GRIP_SN:-}'" \
|
||
-p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'" \
|
||
>>"${log}" 2>&1 < /dev/null &
|
||
local pid=$!
|
||
local i
|
||
for i in $(seq 1 24); do
|
||
if ! kill -0 "${pid}" 2>/dev/null; then
|
||
echo "ERROR: 夹爪驱动起来就退出了。" >&2
|
||
tail -n 30 "${log}" >&2 || true
|
||
return 1
|
||
fi
|
||
if timeout 2 ros2 topic echo /gripper/states --once >/dev/null 2>&1; then
|
||
echo "=== 夹爪驱动已在发 /gripper/states（pid ${pid}）==="
|
||
return 0
|
||
fi
|
||
done
|
||
echo "ERROR: 夹爪驱动没有发出 /gripper/states。" >&2
|
||
tail -n 30 "${log}" >&2 || true
|
||
kill "${pid}" 2>/dev/null || true
|
||
return 1
|
||
}
|
||
|
||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
|
||
ensure_pgea_node || exit 1
|
||
fi
|
||
|
||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]] && pgea_node_up; then
|
||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||
export GRIP_CMD="${CLI[0]}"
|
||
export GRIP_VAL="${CLI[1]:-}"
|
||
export GRIP_FORCE="${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}"
|
||
export GRIP_SPEED="${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}"
|
||
echo "=== 平行爪节点已占 ${SITE_GRIP_PORT:-/dev/ttyUSB0}，命令走 /gripper/ctrl ==="
|
||
CTRL="$(python3 - <<'PY'
|
||
import os
|
||
from arm_application.dh_pgea import ctrl_values
|
||
vals = ctrl_values(
|
||
os.environ.get("GRIP_CMD", ""),
|
||
os.environ.get("GRIP_VAL", ""),
|
||
int(os.environ.get("GRIP_FORCE") or "50"),
|
||
int(os.environ.get("GRIP_SPEED") or "50"),
|
||
)
|
||
if vals is None:
|
||
raise SystemExit(2)
|
||
print(", ".join(str(v) for v in vals))
|
||
PY
|
||
)" && rc=0 || rc=$?
|
||
if [[ "${rc}" -eq 2 ]]; then
|
||
exec timeout 4 ros2 topic echo /gripper/states --once
|
||
fi
|
||
if [[ "${rc}" -ne 0 ]]; then
|
||
echo "ERROR: 无法把夹爪命令转成 /gripper/ctrl" >&2
|
||
exit 1
|
||
fi
|
||
exec timeout 8 ros2 topic pub --once /gripper/ctrl std_msgs/msg/Float32MultiArray "{data: [${CTRL}]}"
|
||
fi
|
||
|
||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
|
||
echo "ERROR: 平行爪命令没有送到 /gripper/ctrl。" >&2
|
||
exit 1
|
||
fi
|
||
|
||
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
|
||
;;
|
||
rotate_by)
|
||
ARGS=(--arm right --test rotate_by)
|
||
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
|
||
if [[ -n "${GRIP_FORCE}" ]]; then ARGS+=(--force "${GRIP_FORCE}"); fi
|
||
if [[ -n "${GRIP_SPEED}" ]]; then ARGS+=(--speed "${GRIP_SPEED}"); fi
|
||
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=${SITE_ROBOT_SHORT_ID:-?} ==="
|
||
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 "${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" --speed "${GRIP_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:-}'"
|