95 lines
2.6 KiB
Bash
95 lines
2.6 KiB
Bash
#!/usr/bin/env bash
|
|
set -euo pipefail
|
|
|
|
usage() {
|
|
cat <<'EOF'
|
|
Usage:
|
|
scripts/go2_start_mapping.sh [--rviz|--no-rviz]
|
|
|
|
Starts SLAM mapping on the wired Go2 interface.
|
|
|
|
Defaults:
|
|
- RViz: auto; enabled only when DISPLAY/WAYLAND_DISPLAY exists
|
|
|
|
Examples:
|
|
scripts/go2_start_mapping.sh
|
|
scripts/go2_start_mapping.sh --rviz
|
|
scripts/go2_start_mapping.sh --no-rviz
|
|
EOF
|
|
}
|
|
|
|
rviz_mode="${GO2_OS_USE_RVIZ:-auto}"
|
|
for arg in "$@"; do
|
|
case "$arg" in
|
|
--rviz) rviz_mode=true ;;
|
|
--no-rviz|--headless) rviz_mode=false ;;
|
|
-h|--help) usage; exit 0 ;;
|
|
*) printf '[go2_start_mapping] unknown argument: %s
|
|
' "$arg" >&2; usage; exit 2 ;;
|
|
esac
|
|
done
|
|
|
|
script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
|
repo_root="$(cd "${script_dir}/.." && pwd)"
|
|
export GO2_OS_ROLE=mapping
|
|
unset GO2_ROS_IFACE
|
|
|
|
if pgrep -f 'ros2 launch go2_navigation2 go2_nav2.launch.py' >/dev/null; then
|
|
printf '[go2_start_mapping] ERROR: Nav2 appears to be running. Stop navigation before mapping.
|
|
' >&2
|
|
pgrep -af 'ros2 launch go2_navigation2 go2_nav2.launch.py' >&2 || true
|
|
exit 3
|
|
fi
|
|
|
|
if pgrep -f 'ros2 launch go2_core go2_start.launch.py' >/dev/null; then
|
|
printf '[go2_start_mapping] ERROR: mapping already appears to be running. Stop it first:
|
|
' >&2
|
|
printf ' scripts/go2_stop_mapping.sh
|
|
' >&2
|
|
pgrep -af 'ros2 launch go2_core go2_start.launch.py' >&2 || true
|
|
exit 4
|
|
fi
|
|
|
|
|
|
# shellcheck disable=SC1091
|
|
source "${script_dir}/go2_env.sh" mapping
|
|
|
|
case "$(printf '%s' "${rviz_mode}" | tr '[:upper:]' '[:lower:]')" in
|
|
auto)
|
|
if [[ -n "${DISPLAY:-}" || -n "${WAYLAND_DISPLAY:-}" ]]; then
|
|
use_rviz=true
|
|
rviz_reason="auto:display-detected"
|
|
else
|
|
use_rviz=false
|
|
rviz_reason="auto:no-display-detected"
|
|
fi
|
|
;;
|
|
1|true|yes|on)
|
|
use_rviz=true
|
|
rviz_reason="forced:true"
|
|
;;
|
|
0|false|no|off)
|
|
use_rviz=false
|
|
rviz_reason="forced:false"
|
|
;;
|
|
*)
|
|
printf '[go2_start_mapping] ERROR: invalid RViz mode: %s (expected auto/true/false)
|
|
' "${rviz_mode}" >&2
|
|
exit 6
|
|
;;
|
|
esac
|
|
|
|
cd "${repo_root}"
|
|
printf '[go2_start_mapping] policy=%s iface=%s reason=%s rviz=%s (%s)
|
|
' "${GO2_TRANSPORT_POLICY:-mapping}" "${GO2_ROS_IFACE}" "${GO2_OS_SELECTED_IFACE_REASON:-n/a}" "${use_rviz}" "${rviz_reason}"
|
|
printf '[go2_start_mapping] starting SLAM mapping on wired iface %s
|
|
' "${GO2_ROS_IFACE}"
|
|
printf '[go2_start_mapping] keep this terminal open while mapping. Press Ctrl+C to stop after saving the map.
|
|
'
|
|
|
|
# 清除 DDS 缓存，避免历史地图残留
|
|
rm -rf /dev/shm/fastrtps_* /dev/shm/cyclonedds_* /tmp/cyclonedds_* 2>/dev/null || true
|
|
printf '[go2_start_mapping] DDS cache cleared — clean start\n'
|
|
|
|
exec ros2 launch go2_core go2_start.launch.py use_rviz:="${use_rviz}"
|