#!/usr/bin/env bash
set -euo pipefail
ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
source "${ROOT_DIR}/scripts/go2_env.sh" nav
if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then
  _restore_nounset=0
  case "$-" in
    *u*) _restore_nounset=1; set +u ;;
  esac
  # colcon-generated setup files may read optional vars such as COLCON_TRACE.
  source "${ROOT_DIR}/install/setup.bash"
  if [[ "${_restore_nounset}" == "1" ]]; then
    set -u
  fi
  unset _restore_nounset
fi
if ros2 service list | grep -Fxq /go2_patrol/clear_route; then
  exec ros2 service call /go2_patrol/clear_route std_srvs/srv/Trigger '{}'
fi
ROUTE_FILE="${GO2_PATROL_ROUTE_FILE:-${ROOT_DIR}/config/ros2_patrol_routes/default.yaml}"
python3 - "${ROUTE_FILE}" <<'PY'
import pathlib
import sys
import time
path = pathlib.Path(sys.argv[1]).expanduser()
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text(
    "route_name: default
"
    "frame_id: map
"
    "created_by: rviz_goal_pose
"
    f"updated_at: {time.strftime('%Y-%m-%dT%H:%M:%S%z')}
"
    "waypoints: []
",
    encoding="utf-8",
)
print(f"cleared route file: {path}")
PY