#!/bin/bash
# 航点标记专用脚本（不启动Nav2导航，机器狗不会动）

echo ==========================================
echo GO2 机器狗航点标记（标点模式）
echo ==========================================
echo 
echo 说明：
echo  - 只启动定位（AMCL），不启动导航控制器
echo  - 在 RViz 中用 Nav2 Goal 标点，机器狗不会移动
echo  - 标完后 Ctrl+C 停止，航点自动保存
echo 
echo 管理命令（新终端）：
echo  - 删除最后一个: ros2 service call /go2_patrol/delete_last_waypoint std_srvs/srv/Trigger
echo  - 清空所有: ros2 service call /go2_patrol/clear_route std_srvs/srv/Trigger
echo  - 查看航点: cat /c/Users/ASUS/go2_robot_system/config/ros2_patrol_routes/default.yaml
echo 

source /opt/ros/humble/setup.bash
source ~/unitree_ros2/cyclonedds_ws/install/setup.bash
source ~/go2_robot_system/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

ros2 launch go2_navigation2 go2_waypoint_mark.launch.py map:=/home/limerly/go2_maps/competition_map.yaml