342 lines
10 KiB
Bash
342 lines
10 KiB
Bash
|
#!/usr/bin/env bash
|
|||
|
|
# Start the real-vehicle Autoware stack and one preset-route tool mode.
|
|||
|
|
#
|
|||
|
|
# 用法:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh [模式] [站点文件] [是否启动RViz] [地图目录] [车型]
|
|||
|
|
#
|
|||
|
|
# 位置参数都可以省略，省略时使用下面的默认值或对应环境变量:
|
|||
|
|
# 模式: ui-only
|
|||
|
|
# 可选 record | replay | mission | none | ui-only
|
|||
|
|
# 站点文件: $PRESET_ROUTE_STATIONS_FILE
|
|||
|
|
# 默认 src/UI/preset_route_cruise/paths/recorded_stations.yaml
|
|||
|
|
# RViz: $PRESET_ROUTE_RVIZ
|
|||
|
|
# 非ui-only模式默认 true；ui-only打开UI时默认 false，可选 true | false
|
|||
|
|
# 地图目录: $PRESET_ROUTE_MAP_PATH
|
|||
|
|
# 默认 /home/nvidia/FaRui-test/map/School06
|
|||
|
|
# 车型: $PRESET_ROUTE_VEHICLE_MODEL
|
|||
|
|
# 默认 ecar_vehicle
|
|||
|
|
#
|
|||
|
|
# 常用示例:
|
|||
|
|
# 只打开UI，不启动主板Autoware:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh
|
|||
|
|
#
|
|||
|
|
# 只启动主板Autoware，不启动预设路线工具:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh none
|
|||
|
|
#
|
|||
|
|
# 启动主板Autoware并关闭RViz:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh mission "" false
|
|||
|
|
#
|
|||
|
|
# 使用指定地图:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh mission "" true /home/nvidia/FaRui-test/map/School06
|
|||
|
|
#
|
|||
|
|
# 启动主板Autoware + 站点任务管理器:
|
|||
|
|
# ./run_preset_route_real_vehicle.sh mission
|
|||
|
|
#
|
|||
|
|
# 常用环境变量:
|
|||
|
|
# PRESET_ROUTE_MAP_PATH=/path/to/map
|
|||
|
|
# PRESET_ROUTE_RVIZ=false
|
|||
|
|
# PRESET_ROUTE_NO_UI=true
|
|||
|
|
# PRESET_ROUTE_SKIP_MAIN_STACK=true
|
|||
|
|
# PRESET_ROUTE_START_ON_LAUNCH=true
|
|||
|
|
# PRESET_ROUTE_LOOP_MISSION=true
|
|||
|
|
# PRESET_ROUTE_START_STATION_INDEX=0
|
|||
|
|
# PRESET_ROUTE_AUTOWARE_LAUNCH_FILE=/path/to/ecar_master.launch.xml
|
|||
|
|
#
|
|||
|
|
# 双Orin说明:
|
|||
|
|
# 本脚本用于主板，默认启动 src/Launcher/farui_launch/launch/ecar_master.launch.xml。
|
|||
|
|
# 主板默认不启动 system、vehicle、control；这些由从板脚本负责。
|
|||
|
|
|
|||
|
|
set -euo pipefail
|
|||
|
|
|
|||
|
|
GREEN='\033[0;32m'
|
|||
|
|
YELLOW='\033[1;33m'
|
|||
|
|
RED='\033[0;31m'
|
|||
|
|
NC='\033[0m'
|
|||
|
|
|
|||
|
|
log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
|
|||
|
|
log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
|
|||
|
|
log_error() { echo -e "${RED}[ERROR]${NC} $*" >&2; }
|
|||
|
|
|
|||
|
|
script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
|||
|
|
|
|||
|
|
find_workspace_root() {
|
|||
|
|
if [[ -n "${PRESET_ROUTE_WORKSPACE_ROOT:-}" ]]; then
|
|||
|
|
echo "${PRESET_ROUTE_WORKSPACE_ROOT}"
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
local candidates=(
|
|||
|
|
"${PWD}"
|
|||
|
|
"${script_dir}"
|
|||
|
|
"${script_dir}/../../../.."
|
|||
|
|
"${script_dir}/../../../../.."
|
|||
|
|
)
|
|||
|
|
for candidate in "${candidates[@]}"; do
|
|||
|
|
if [[ -f "${candidate}/install/setup.bash" ]]; then
|
|||
|
|
(cd "${candidate}" && pwd)
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
done
|
|||
|
|
|
|||
|
|
echo "${PWD}"
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
kill_process_group() {
|
|||
|
|
local pid="$1"
|
|||
|
|
local signal="${2:-TERM}"
|
|||
|
|
|
|||
|
|
if [[ -z "${pid}" ]] || ! kill -0 "${pid}" 2>/dev/null; then
|
|||
|
|
return 0
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
local pgid
|
|||
|
|
pgid="$(ps -o pgid= -p "${pid}" 2>/dev/null | tr -d ' ')"
|
|||
|
|
|
|||
|
|
if [[ -n "${pgid}" ]]; then
|
|||
|
|
kill "-${signal}" -- "-${pgid}" 2>/dev/null || true
|
|||
|
|
else
|
|||
|
|
kill "-${signal}" "${pid}" 2>/dev/null || true
|
|||
|
|
fi
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
launch_preset_route_ui() {
|
|||
|
|
local source_ui="${WORKSPACE_ROOT}/src/UI/preset_route_cruise/scripts/preset_route_operator.py"
|
|||
|
|
if [[ -f "${source_ui}" ]]; then
|
|||
|
|
log_info "Launching preset route UI from source."
|
|||
|
|
python3 "${source_ui}"
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
local installed_ui="${WORKSPACE_ROOT}/install/preset_route_cruise/lib/preset_route_cruise/preset_route_operator.py"
|
|||
|
|
if [[ -f "${installed_ui}" ]]; then
|
|||
|
|
log_warn "preset_route_operator.py is not registered as a ros2 executable; launching installed script."
|
|||
|
|
python3 "${installed_ui}"
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if ros2 pkg executables preset_route_cruise 2>/dev/null | awk '{print $2}' | grep -qx "preset_route_operator.py"; then
|
|||
|
|
ros2 run preset_route_cruise preset_route_operator.py
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
log_error "Preset route UI executable not found. Expected ros2 executable, ${source_ui}, or ${installed_ui}"
|
|||
|
|
return 1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
find_default_map_path() {
|
|||
|
|
echo "/home/nvidia/FaRui-test/map/School06"
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
WORKSPACE_ROOT="$(find_workspace_root)"
|
|||
|
|
DEFAULT_STATIONS_FILE="${WORKSPACE_ROOT}/src/UI/preset_route_cruise/paths/recorded_stations.yaml"
|
|||
|
|
DEFAULT_MAP_PATH="$(find_default_map_path)"
|
|||
|
|
DEFAULT_VEHICLE_MODEL="ecar_vehicle"
|
|||
|
|
DEFAULT_AUTOWARE_LAUNCH_FILE="${WORKSPACE_ROOT}/src/Launcher/farui_launch/launch/ecar_master.launch.xml"
|
|||
|
|
|
|||
|
|
ROUTE_MODE="${1:-ui-only}"
|
|||
|
|
STATIONS_FILE="${2:-${PRESET_ROUTE_STATIONS_FILE:-${DEFAULT_STATIONS_FILE}}}"
|
|||
|
|
RVIZ="${3:-${PRESET_ROUTE_RVIZ:-true}}"
|
|||
|
|
MAP_PATH="${4:-${PRESET_ROUTE_MAP_PATH:-${DEFAULT_MAP_PATH}}}"
|
|||
|
|
VEHICLE_MODEL="${5:-${PRESET_ROUTE_VEHICLE_MODEL:-${DEFAULT_VEHICLE_MODEL}}}"
|
|||
|
|
|
|||
|
|
UI_START_RECORDING_ON_LAUNCH="${PRESET_ROUTE_START_RECORDING_ON_LAUNCH:-false}"
|
|||
|
|
UI_AUTO_SEND_ON_START="${PRESET_ROUTE_AUTO_SEND_ON_START:-false}"
|
|||
|
|
UI_MISSION_START_ON_LAUNCH="${PRESET_ROUTE_START_ON_LAUNCH:-false}"
|
|||
|
|
UI_ALLOW_GOAL_MODIFICATION="${PRESET_ROUTE_ALLOW_GOAL_MODIFICATION:-false}"
|
|||
|
|
UI_MISSION_LOOP="${PRESET_ROUTE_LOOP_MISSION:-false}"
|
|||
|
|
UI_MISSION_START_STATION_INDEX="${PRESET_ROUTE_START_STATION_INDEX:-0}"
|
|||
|
|
UI_SKIP_MAIN_STACK="${PRESET_ROUTE_SKIP_MAIN_STACK:-false}"
|
|||
|
|
UI_RECORD_CLEAR_ON_START="${PRESET_ROUTE_RECORD_CLEAR_ON_START:-false}"
|
|||
|
|
UI_RECORD_APPEND_EXISTING="${PRESET_ROUTE_RECORD_APPEND_EXISTING:-true}"
|
|||
|
|
AUTOWARE_LAUNCH_FILE="${PRESET_ROUTE_AUTOWARE_LAUNCH_FILE:-${DEFAULT_AUTOWARE_LAUNCH_FILE}}"
|
|||
|
|
|
|||
|
|
case "${ROUTE_MODE}" in
|
|||
|
|
record|replay|mission|none|ui-only)
|
|||
|
|
;;
|
|||
|
|
*)
|
|||
|
|
log_error "Unsupported route mode: ${ROUTE_MODE}. Expected record, replay, mission, none, or ui-only."
|
|||
|
|
exit 1
|
|||
|
|
;;
|
|||
|
|
esac
|
|||
|
|
|
|||
|
|
case "${RVIZ}" in
|
|||
|
|
true|false)
|
|||
|
|
;;
|
|||
|
|
*)
|
|||
|
|
log_error "RVIZ must be true or false, got: ${RVIZ}"
|
|||
|
|
exit 1
|
|||
|
|
;;
|
|||
|
|
esac
|
|||
|
|
|
|||
|
|
if [[ ("${ROUTE_MODE}" == "replay" || "${ROUTE_MODE}" == "mission") && ! -f "${STATIONS_FILE}" ]]; then
|
|||
|
|
log_error "Stations file not found: ${STATIONS_FILE}"
|
|||
|
|
exit 1
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${UI_SKIP_MAIN_STACK}" != "true" && ! -d "${MAP_PATH}" ]]; then
|
|||
|
|
log_error "Map path not found: ${MAP_PATH}"
|
|||
|
|
exit 1
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
|
|||
|
|
log_error "Workspace setup file not found: ${WORKSPACE_ROOT}/install/setup.bash"
|
|||
|
|
exit 1
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${UI_SKIP_MAIN_STACK}" != "true" && "${ROUTE_MODE}" != "ui-only" && ! -f "${AUTOWARE_LAUNCH_FILE}" ]]; then
|
|||
|
|
log_error "Autoware launch file not found: ${AUTOWARE_LAUNCH_FILE}"
|
|||
|
|
exit 1
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
log_info "Workspace: ${WORKSPACE_ROOT}"
|
|||
|
|
log_info "Mode: ${ROUTE_MODE}, stations: ${STATIONS_FILE}"
|
|||
|
|
log_info "Autoware launch: ${AUTOWARE_LAUNCH_FILE}"
|
|||
|
|
log_info "Map path: ${MAP_PATH}"
|
|||
|
|
|
|||
|
|
cd "${WORKSPACE_ROOT}"
|
|||
|
|
set +u
|
|||
|
|
source install/setup.bash
|
|||
|
|
set -u
|
|||
|
|
|
|||
|
|
AUTOWARE_STACK_PID=""
|
|||
|
|
EXTRA_TOOL_PID=""
|
|||
|
|
UI_PID=""
|
|||
|
|
|
|||
|
|
if [[ "${UI_SKIP_MAIN_STACK}" != "true" && "${ROUTE_MODE}" != "ui-only" ]]; then
|
|||
|
|
log_info "Launching master Orin Autoware stack."
|
|||
|
|
setsid ros2 launch "${AUTOWARE_LAUNCH_FILE}" \
|
|||
|
|
map_path:="${MAP_PATH}" \
|
|||
|
|
vehicle_model:="${VEHICLE_MODEL}" \
|
|||
|
|
launch_planning:=true \
|
|||
|
|
launch_system:=false \
|
|||
|
|
launch_vehicle:=false \
|
|||
|
|
launch_control:=false \
|
|||
|
|
launch_api:=true \
|
|||
|
|
use_sim_time:=false \
|
|||
|
|
rviz:="${RVIZ}" \
|
|||
|
|
rviz_respawn:=false &
|
|||
|
|
AUTOWARE_STACK_PID=$!
|
|||
|
|
else
|
|||
|
|
log_info "Main stack launch skipped by PRESET_ROUTE_SKIP_MAIN_STACK=true."
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${ROUTE_MODE}" != "ui-only" ]]; then
|
|||
|
|
if [[ "${ROUTE_MODE}" == "record" ]]; then
|
|||
|
|
log_info "Launching station recorder."
|
|||
|
|
setsid ros2 launch preset_route_cruise record_stations_from_rviz.launch.xml \
|
|||
|
|
output_file:="${STATIONS_FILE}" \
|
|||
|
|
clear_file_on_start:="${UI_RECORD_CLEAR_ON_START}" \
|
|||
|
|
append_existing:="${UI_RECORD_APPEND_EXISTING}" \
|
|||
|
|
write_on_each_point:=true \
|
|||
|
|
start_recording_on_launch:="${UI_START_RECORDING_ON_LAUNCH}" \
|
|||
|
|
use_sim_time:=false &
|
|||
|
|
EXTRA_TOOL_PID=$!
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${ROUTE_MODE}" == "replay" ]]; then
|
|||
|
|
log_info "Launching preset route sender."
|
|||
|
|
setsid ros2 launch preset_route_cruise preset_route_cruise.launch.xml \
|
|||
|
|
stations_file:="${STATIONS_FILE}" \
|
|||
|
|
clear_route_before_send:=true \
|
|||
|
|
allow_goal_modification:="${UI_ALLOW_GOAL_MODIFICATION}" \
|
|||
|
|
auto_shutdown_after_send:=false \
|
|||
|
|
auto_send_on_start:="${UI_AUTO_SEND_ON_START}" \
|
|||
|
|
use_sim_time:=false &
|
|||
|
|
EXTRA_TOOL_PID=$!
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${ROUTE_MODE}" == "mission" ]]; then
|
|||
|
|
log_info "Launching station mission manager."
|
|||
|
|
setsid ros2 launch preset_route_cruise station_mission_manager.launch.xml \
|
|||
|
|
stations_file:="${STATIONS_FILE}" \
|
|||
|
|
loop_mission:="${UI_MISSION_LOOP}" \
|
|||
|
|
auto_start_on_each_leg:=false \
|
|||
|
|
manual_start_first_leg_only:=true \
|
|||
|
|
clear_route_before_send:=true \
|
|||
|
|
allow_goal_modification:="${UI_ALLOW_GOAL_MODIFICATION}" \
|
|||
|
|
start_on_launch:="${UI_MISSION_START_ON_LAUNCH}" \
|
|||
|
|
start_station_index:="${UI_MISSION_START_STATION_INDEX}" \
|
|||
|
|
auto_shutdown_on_finish:=false \
|
|||
|
|
use_sim_time:=false &
|
|||
|
|
EXTRA_TOOL_PID=$!
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ "${ROUTE_MODE}" == "none" ]]; then
|
|||
|
|
log_info "Extra preset-route tool launch skipped."
|
|||
|
|
fi
|
|||
|
|
else
|
|||
|
|
log_info "UI-only mode: main stack and tools will be started from UI."
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
# 启动 UI 界面
|
|||
|
|
if [[ "${PRESET_ROUTE_NO_UI:-false}" != "true" ]]; then
|
|||
|
|
export PRESET_ROUTE_MAP_PATH="${MAP_PATH}"
|
|||
|
|
export PRESET_ROUTE_STATIONS_FILE="${STATIONS_FILE}"
|
|||
|
|
export PRESET_ROUTE_VEHICLE_MODEL="${VEHICLE_MODEL}"
|
|||
|
|
if [[ "${ROUTE_MODE}" == "ui-only" ]]; then
|
|||
|
|
log_info "Launching preset route UI (UI-only mode)."
|
|||
|
|
PRESET_ROUTE_RVIZ="${PRESET_ROUTE_RVIZ:-false}" launch_preset_route_ui &
|
|||
|
|
UI_PID=$!
|
|||
|
|
else
|
|||
|
|
UI_LAUNCH_DELAY="${PRESET_ROUTE_UI_LAUNCH_DELAY:-3}"
|
|||
|
|
log_info "UI will launch in ${UI_LAUNCH_DELAY} seconds..."
|
|||
|
|
(
|
|||
|
|
sleep "${UI_LAUNCH_DELAY}"
|
|||
|
|
log_info "Launching preset route UI."
|
|||
|
|
launch_preset_route_ui
|
|||
|
|
) &
|
|||
|
|
UI_PID=$!
|
|||
|
|
fi
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
cleanup() {
|
|||
|
|
trap - EXIT INT TERM
|
|||
|
|
|
|||
|
|
local pids=()
|
|||
|
|
if [[ -n "${UI_PID}" ]]; then
|
|||
|
|
pids+=("${UI_PID}")
|
|||
|
|
fi
|
|||
|
|
if [[ -n "${EXTRA_TOOL_PID}" ]]; then
|
|||
|
|
pids+=("${EXTRA_TOOL_PID}")
|
|||
|
|
fi
|
|||
|
|
if [[ -n "${AUTOWARE_STACK_PID}" ]]; then
|
|||
|
|
pids+=("${AUTOWARE_STACK_PID}")
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ ${#pids[@]} -eq 0 ]]; then
|
|||
|
|
return
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
log_info "Shutting down launched processes."
|
|||
|
|
|
|||
|
|
for pid in "${pids[@]}"; do
|
|||
|
|
kill_process_group "${pid}" TERM
|
|||
|
|
done
|
|||
|
|
|
|||
|
|
sleep 2
|
|||
|
|
|
|||
|
|
for pid in "${pids[@]}"; do
|
|||
|
|
kill_process_group "${pid}" KILL
|
|||
|
|
done
|
|||
|
|
|
|||
|
|
wait "${pids[@]}" 2>/dev/null || true
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
trap cleanup EXIT INT TERM
|
|||
|
|
|
|||
|
|
WAIT_PIDS=()
|
|||
|
|
if [[ -n "${AUTOWARE_STACK_PID}" ]]; then
|
|||
|
|
WAIT_PIDS+=("${AUTOWARE_STACK_PID}")
|
|||
|
|
fi
|
|||
|
|
if [[ -n "${EXTRA_TOOL_PID}" ]]; then
|
|||
|
|
WAIT_PIDS+=("${EXTRA_TOOL_PID}")
|
|||
|
|
fi
|
|||
|
|
if [[ -n "${UI_PID}" ]]; then
|
|||
|
|
WAIT_PIDS+=("${UI_PID}")
|
|||
|
|
fi
|
|||
|
|
|
|||
|
|
if [[ ${#WAIT_PIDS[@]} -gt 0 ]]; then
|
|||
|
|
wait "${WAIT_PIDS[@]}"
|
|||
|
|
fi
|
|||
|
|
|