173 lines
5.2 KiB
Bash
173 lines
5.2 KiB
Bash
#!/usr/bin/env bash
|
||
# 单终端一键：按需 roscore → MAVROS 串口(MAVLink) → px4_ctrl_offboard_demo.py
|
||
#
|
||
# 用法（在仓库根目录）：
|
||
# bash scripts/run_px4_offboard_one_terminal.sh
|
||
# bash scripts/run_px4_offboard_one_terminal.sh /dev/ttyACM0 921600
|
||
# bash scripts/run_px4_offboard_one_terminal.sh /dev/ttyACM0 921600 120
|
||
# 第三参 DEMO_MAX_SEC：仅限制「px4_ctrl_offboard_demo.py」运行时长（秒），到时发 SIGTERM 结束
|
||
# demo，随后脚本清理 MAVROS / 可选 roscore 并退出；0 或不写表示不限制。
|
||
# 也可用环境变量 OFFBOARD_DEMO_MAX_SEC（第三参优先）。
|
||
#
|
||
# 环境变量（可选）：
|
||
# ROS_MASTER_URI 默认 http://127.0.0.1:11311
|
||
# ROS_HOSTNAME 默认 127.0.0.1
|
||
# OFFBOARD_PYTHON 默认优先用 conda env「yanshi」里的 python，否则 python3
|
||
# OFFBOARD_DEMO_MAX_SEC 同第三参；整条脚本从头限时请用: timeout 600 bash 本脚本 ...
|
||
# ROCKET_OFFBOARD_DEMO_PY 显式指定 px4_ctrl_offboard_demo.py 路径（缺省时先 $ROOT/src/ 再 $ROOT/../src/）
|
||
#
|
||
# 退出：Ctrl+C 会结束 demo，并停止本脚本拉起的 MAVROS；若 roscore 由本脚本启动，会一并结束。
|
||
|
||
set -euo pipefail
|
||
|
||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||
ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
|
||
cd "$ROOT"
|
||
|
||
resolve_demo_py() {
|
||
if [[ -n "${ROCKET_OFFBOARD_DEMO_PY:-}" ]] && [[ -f "${ROCKET_OFFBOARD_DEMO_PY}" ]]; then
|
||
echo "$(cd "$(dirname "${ROCKET_OFFBOARD_DEMO_PY}")" && pwd)/$(basename "${ROCKET_OFFBOARD_DEMO_PY}")"
|
||
return
|
||
fi
|
||
local c1="$ROOT/src/px4_ctrl_offboard_demo.py"
|
||
if [[ -f "$c1" ]]; then
|
||
echo "$c1"
|
||
return
|
||
fi
|
||
local c2="$ROOT/../src/px4_ctrl_offboard_demo.py"
|
||
if [[ -f "$c2" ]]; then
|
||
echo "$(cd "$(dirname "$c2")" && pwd)/px4_ctrl_offboard_demo.py"
|
||
return
|
||
fi
|
||
echo "错误: 找不到 px4_ctrl_offboard_demo.py。已尝试:" >&2
|
||
echo " ROCKET_OFFBOARD_DEMO_PY=${ROCKET_OFFBOARD_DEMO_PY:-（未设置）}" >&2
|
||
echo " $c1" >&2
|
||
echo " $c2" >&2
|
||
exit 3
|
||
}
|
||
|
||
DEMO_PY="$(resolve_demo_py)"
|
||
|
||
source /opt/ros/noetic/setup.bash
|
||
export ROS_MASTER_URI="${ROS_MASTER_URI:-http://127.0.0.1:11311}"
|
||
export ROS_HOSTNAME="${ROS_HOSTNAME:-127.0.0.1}"
|
||
|
||
DEV="${1:-/dev/ttyACM0}"
|
||
BAUD="${2:-921600}"
|
||
DEMO_MAX_SEC="${3:-${OFFBOARD_DEMO_MAX_SEC:-0}}"
|
||
FCU_URL="${DEV}:${BAUD}"
|
||
|
||
if ! [[ "$DEMO_MAX_SEC" =~ ^[0-9]+$ ]]; then
|
||
echo "错误: 第三参（demo 最长运行秒数）须为非负整数，当前=${DEMO_MAX_SEC}"
|
||
exit 2
|
||
fi
|
||
|
||
ROSCORE_PID=""
|
||
MAVROS_PID=""
|
||
WE_STARTED_ROSCORE=0
|
||
|
||
master_ok() {
|
||
timeout 3 rosnode list &>/dev/null
|
||
}
|
||
|
||
stop_mavros() {
|
||
pkill -f '/opt/ros/noetic/lib/mavros/mavros_node' 2>/dev/null || true
|
||
}
|
||
|
||
kill_children() {
|
||
if [[ -n "${MAVROS_PID:-}" ]] && kill -0 "$MAVROS_PID" 2>/dev/null; then
|
||
kill "$MAVROS_PID" 2>/dev/null || true
|
||
wait "$MAVROS_PID" 2>/dev/null || true
|
||
fi
|
||
if [[ "${WE_STARTED_ROSCORE}" -eq 1 ]] && [[ -n "${ROSCORE_PID:-}" ]] && kill -0 "$ROSCORE_PID" 2>/dev/null; then
|
||
kill "$ROSCORE_PID" 2>/dev/null || true
|
||
wait "$ROSCORE_PID" 2>/dev/null || true
|
||
fi
|
||
}
|
||
|
||
trap 'kill_children; exit 130' INT
|
||
trap 'kill_children; exit 143' TERM
|
||
|
||
resolve_python() {
|
||
if [[ -n "${OFFBOARD_PYTHON:-}" ]] && [[ -x "$OFFBOARD_PYTHON" ]]; then
|
||
echo "$OFFBOARD_PYTHON"
|
||
return
|
||
fi
|
||
for cand in \
|
||
"${HOME}/miniconda3/envs/yanshi/bin/python" \
|
||
"${HOME}/anaconda3/envs/yanshi/bin/python" \
|
||
"${HOME}/mambaforge/envs/yanshi/bin/python"; do
|
||
if [[ -x "$cand" ]]; then
|
||
echo "$cand"
|
||
return
|
||
fi
|
||
done
|
||
command -v python3
|
||
}
|
||
|
||
if ! master_ok; then
|
||
echo "[1/3] 启动 roscore ..."
|
||
roscore > /tmp/roscore_offboard_one_terminal.log 2>&1 &
|
||
ROSCORE_PID=$!
|
||
WE_STARTED_ROSCORE=1
|
||
for _ in $(seq 1 50); do
|
||
master_ok && break
|
||
sleep 0.2
|
||
done
|
||
if ! master_ok; then
|
||
echo "roscore 未起来，请查看: tail -40 /tmp/roscore_offboard_one_terminal.log"
|
||
kill_children
|
||
exit 1
|
||
fi
|
||
echo "roscore 已就绪 (pid=$ROSCORE_PID)"
|
||
else
|
||
echo "[1/3] 已有 ROS master，跳过 roscore"
|
||
fi
|
||
|
||
echo "[2/3] 启动 MAVROS fcu_url=${FCU_URL}"
|
||
stop_mavros
|
||
sleep 1
|
||
roslaunch mavros px4.launch fcu_url:="$FCU_URL" > /tmp/mavros_offboard_one_terminal.log 2>&1 &
|
||
MAVROS_PID=$!
|
||
|
||
echo "等待飞控连接 (connected=true)，超时 60s ..."
|
||
connected=0
|
||
for _ in $(seq 1 60); do
|
||
if timeout 4 rostopic echo /mavros/state -n 1 2>/dev/null | grep -qE 'connected: [Tt]rue'; then
|
||
connected=1
|
||
break
|
||
fi
|
||
sleep 1
|
||
done
|
||
if [[ "$connected" -ne 1 ]]; then
|
||
echo "仍未连上飞控。请检查串口与波特率，日志: tail -60 /tmp/mavros_offboard_one_terminal.log"
|
||
echo "可重试: bash $0 ${DEV} 57600"
|
||
kill_children
|
||
exit 1
|
||
fi
|
||
echo "MAVROS 已连接飞控"
|
||
|
||
PY="$(resolve_python)"
|
||
echo "[3/3] 使用 Python: $PY"
|
||
if [[ "$DEMO_MAX_SEC" -gt 0 ]]; then
|
||
echo "运行 $DEMO_PY，${DEMO_MAX_SEC}s 后自动结束 demo 并退出本脚本（随后清理子进程）"
|
||
else
|
||
echo "运行 $DEMO_PY （Ctrl+C 结束并将停止本脚本启动的 MAVROS）"
|
||
fi
|
||
set +e
|
||
if [[ "$DEMO_MAX_SEC" -gt 0 ]]; then
|
||
timeout --signal=TERM --kill-after=8 "$DEMO_MAX_SEC" \
|
||
"$PY" "$DEMO_PY"
|
||
DEMO_EXIT=$?
|
||
if [[ "$DEMO_EXIT" -eq 124 ]]; then
|
||
echo "[timeout] 已到 ${DEMO_MAX_SEC}s，已终止 px4_ctrl_offboard_demo.py（退出码 124）"
|
||
fi
|
||
else
|
||
"$PY" "$DEMO_PY"
|
||
DEMO_EXIT=$?
|
||
fi
|
||
set -e
|
||
|
||
kill_children
|
||
exit "$DEMO_EXIT"
|