#!/usr/bin/env bash
# MuJoCo G20 右手仿真桥
# 默认订阅 /cb_right_hand_control_cmd（0~255，与真机 SDK 一致）
# 发布 /sim/mujoco/g20/right/joint_state
# 模型：urdf/G20/linker_hand_g20_right/linker_hand_g20_right.xml
# 默认 1.35× 墙钟 + 关节限速；可用 --speed / --slew-rad-s 再调
#
# 用法:
#   ./hands/G20/scripts/run_g20_right_mujoco_plan.sh
#   ./hands/G20/scripts/run_g20_right_mujoco_plan.sh --cmd-topic /g20/cb_right_hand_control_cmd
# 监视关节（另开终端）:
#   ./hands/G20/scripts/run_g20_right_joint_gui.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"

set +u
# shellcheck disable=SC1091
source /opt/ros/jazzy/setup.bash
if [[ -f "$ROOT/install/setup.bash" ]]; then
  # shellcheck disable=SC1091
  source "$ROOT/install/setup.bash"
fi
set -u

VENV_SITE="$ROOT/venv/lib/python3.12/site-packages"
if [[ -d "$VENV_SITE" ]]; then
  export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}"
fi

# 源码优先（含 urdf 资产）
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"

XML="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_right/linker_hand_g20_right.xml"
if [[ ! -f "$XML" ]]; then
  echo "[error] missing G20 right MJCF: $XML" >&2
  exit 1
fi

echo "[g20-right] xml=$XML"
echo "[g20-right] model=G20_RIGHT_tag"
echo "[g20-right] cmd-topic=/cb_right_hand_control_cmd (override with --cmd-topic)"
echo "[g20-right] sim-state=/sim/mujoco/g20/right/joint_state"

exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge \
  --xml "$XML" \
  --node-name g20_mujoco_plan_bridge_right \
  --window-title "G20 right" \
  --cmd-topic /cb_right_hand_control_cmd \
  --sim-state-topic /sim/mujoco/g20/right/joint_state \
  --nominal-topic /retarget/g20/right/joint_target_nominal \
  --mapping g20_urdf \
  "$@"