feat: 初始化 Go2 导航相关代码与配置

go2_autonomous_navigation/scripts/go2_clear_waypoints.sh

@@ -0,0 +1,41 @@
+#!/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