97 lines
2.8 KiB
Bash
97 lines
2.8 KiB
Bash
|
#!/usr/bin/env bash
|
||
|
|
set -euo pipefail
|
||
|
|
|
||
|
|
usage() {
|
||
|
|
cat <<'EOF'
|
||
|
|
Usage:
|
||
|
|
scripts/go2_start_nav.sh [map_name_or_yaml]
|
||
|
|
|
||
|
|
Examples:
|
||
|
|
scripts/go2_start_nav.sh
|
||
|
|
scripts/go2_start_nav.sh competition_map
|
||
|
|
scripts/go2_start_nav.sh ~/go2_maps/final_site.yaml
|
||
|
|
|
||
|
|
Starts Nav2 with the specified map. In RViz, set 2D Pose Estimate first, then
|
||
|
|
send a Nav2 Goal.
|
||
|
|
GO2_OS_USE_RVIZ=auto|true|false overrides RViz launch (default: auto).
|
||
|
|
EOF
|
||
|
|
}
|
||
|
|
|
||
|
|
if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then
|
||
|
|
usage
|
||
|
|
exit 0
|
||
|
|
fi
|
||
|
|
|
||
|
|
script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||
|
|
repo_root="$(cd "${script_dir}/.." && pwd)"
|
||
|
|
export GO2_OS_ROLE=nav
|
||
|
|
unset GO2_ROS_IFACE
|
||
|
|
|
||
|
|
if pgrep -f 'ros2 launch go2_core go2_start.launch.py' >/dev/null; then
|
||
|
|
printf '[go2_start_nav] ERROR: mapping launch appears to be running. Stop mapping before Nav2.\n' >&2
|
||
|
|
pgrep -af 'ros2 launch go2_core go2_start.launch.py' >&2 || true
|
||
|
|
exit 3
|
||
|
|
fi
|
||
|
|
|
||
|
|
if pgrep -f 'ros2 launch go2_navigation2 go2_nav2.launch.py' >/dev/null; then
|
||
|
|
printf '[go2_start_nav] ERROR: Nav2 already appears to be running.\n' >&2
|
||
|
|
pgrep -af 'ros2 launch go2_navigation2 go2_nav2.launch.py' >&2 || true
|
||
|
|
exit 4
|
||
|
|
fi
|
||
|
|
|
||
|
|
# shellcheck disable=SC1091
|
||
|
|
source "${script_dir}/go2_env.sh"
|
||
|
|
|
||
|
|
map_arg="${1:-${GO2_OS_MAP_NAME}}"
|
||
|
|
if [[ "${map_arg}" == "~" ]]; then
|
||
|
|
map_arg="${HOME}"
|
||
|
|
elif [[ "${map_arg}" == "~/"* ]]; then
|
||
|
|
map_arg="${HOME}/${map_arg#"~/"}"
|
||
|
|
fi
|
||
|
|
|
||
|
|
if [[ "${map_arg}" == *.yaml ]]; then
|
||
|
|
map_yaml="${map_arg}"
|
||
|
|
elif [[ "${map_arg}" == /* || "${map_arg}" == ~* || "${map_arg}" == *"/"* ]]; then
|
||
|
|
map_yaml="${map_arg}.yaml"
|
||
|
|
else
|
||
|
|
map_yaml="${GO2_OS_MAP_DIR}/${map_arg}.yaml"
|
||
|
|
fi
|
||
|
|
|
||
|
|
if [[ ! -f "${map_yaml}" ]]; then
|
||
|
|
printf '[go2_start_nav] ERROR: map yaml not found: %s\n' "${map_yaml}" >&2
|
||
|
|
exit 5
|
||
|
|
fi
|
||
|
|
|
||
|
|
rviz_mode="${GO2_OS_USE_RVIZ:-auto}"
|
||
|
|
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_nav] ERROR: invalid GO2_OS_USE_RVIZ=%s (expected auto/true/false)\n' "${GO2_OS_USE_RVIZ:-}" >&2
|
||
|
|
exit 6
|
||
|
|
;;
|
||
|
|
esac
|
||
|
|
|
||
|
|
cd "${repo_root}"
|
||
|
|
printf '[go2_start_nav] policy=%s iface=%s reason=%s rviz=%s (%s)\n' "${GO2_TRANSPORT_POLICY:-nav}" "${GO2_ROS_IFACE}" "${GO2_OS_SELECTED_IFACE_REASON:-n/a}" "${use_rviz}" "${rviz_reason}"
|
||
|
|
if [[ "${GO2_OS_FALLBACK_ACTIVE:-0}" == "1" ]]; then
|
||
|
|
printf '[go2_start_nav] fallback active: wired backup in use\n'
|
||
|
|
fi
|
||
|
|
printf '[go2_start_nav] starting Nav2 on LAN iface %s with map: %s\n' "${GO2_ROS_IFACE}" "${map_yaml}"
|
||
|
|
exec ros2 launch go2_navigation2 go2_nav2.launch.py map:="${map_yaml}" use_rviz:="${use_rviz}"
|