#!/usr/bin/env bash
# MuJoCo G20 左手仿真桥
# 默认订阅 /g20/cb_left_hand_control_cmd（0~255）
# 发布 /sim/mujoco/g20/left/joint_state
# 默认 1.35× 墙钟 + 关节限速；可用 --speed / --slew-rad-s 再调
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}"

exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge "$@"