#!/usr/bin/env bash
set -eo pipefail

APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
source /opt/ros/jazzy/setup.bash
if [[ -f /home/nvidia/xos/setup.bash ]]; then
  source /home/nvidia/xos/setup.bash
fi
if [[ -f /opt/robot_tele_server/install/setup.bash ]]; then
  source /opt/robot_tele_server/install/setup.bash
fi
if [[ -f "$APP_DIR/ros2_py/install/setup.bash" ]]; then
  source "$APP_DIR/ros2_py/install/setup.bash"
fi
set -u

case "${1:-status}" in
  start)
    ros2 service call /tg3_local_teleop/return_home std_srvs/srv/Trigger "{}"
    ;;
  cancel|stop)
    ros2 service call /tg3_local_teleop/cancel_home std_srvs/srv/Trigger "{}"
    ;;
  status)
    "$APP_DIR/status.sh"
    ;;
  *)
    echo "usage: $0 {start|cancel|status}" >&2
    exit 2
    ;;
esac