#!/usr/bin/env bash
# 把两个 DH 夹爪（左 PGEA + 右 RGI）作为同一个 ROS 2 节点启动。
#   bash ~/ros2/scripts/run_dual_gripper.sh
# 话题：/dual_gripper_driver/left|right/{command,status}
# 服务：/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