arm_dev
144 lines
5.1 KiB
Bash
Executable File
144 lines
5.1 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"
|
||
|
||
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:-}'"
|