147 lines
4.4 KiB
Bash
147 lines
4.4 KiB
Bash
#!/usr/bin/env bash
|
||
# 一键：roscore（若还没有）→ MAVROS（串口连 PX4）→ flight_intent 伴飞桥（前台，直到 Ctrl+C）
|
||
#
|
||
# 适合「不想自己敲 roslaunch」时直接跑；用法与 run_px4_offboard_one_terminal.sh 前两参一致。
|
||
#
|
||
# 在 voice_drone_assistant 根目录：
|
||
# bash scripts/run_flight_bridge_with_mavros.sh
|
||
# bash scripts/run_flight_bridge_with_mavros.sh /dev/ttyACM0 921600
|
||
#
|
||
# 飞控连上后，另开终端发 JSON（std_msgs/String 必须用 YAML 字段 data，见 docs/FLIGHT_BRIDGE_ROS1.md）：
|
||
# source /opt/ros/noetic/setup.bash
|
||
# rostopic pub -1 /input std_msgs/String \
|
||
# "data: '{\"is_flight_intent\":true,\"version\":1,\"actions\":[{\"type\":\"land\",\"args\":{}}],\"summary\":\"降\"}'"
|
||
#
|
||
# 环境变量（可选）：
|
||
# ROS_MASTER_URI 默认 http://127.0.0.1:11311
|
||
# ROS_HOSTNAME 默认 127.0.0.1
|
||
# BRIDGE_PYTHON 若不设则 python3（与 mavros 同机即可，不必 conda）
|
||
# OFFBOARD_PYTHON 未设 BRIDGE_PYTHON 时也试 yanshi 环境（与 offboard 脚本习惯一致）
|
||
#
|
||
# Ctrl+C：结束伴飞桥，并停止本脚本拉起的 MAVROS；若 roscore 由本脚本启动则一并结束。
|
||
|
||
set -euo pipefail
|
||
|
||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||
ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
|
||
cd "$ROOT"
|
||
|
||
if [[ ! -f /opt/ros/noetic/setup.bash ]]; then
|
||
echo "未找到 /opt/ros/noetic/setup.bash（伴飞桥当前仅支持 ROS1 Noetic）" >&2
|
||
exit 2
|
||
fi
|
||
|
||
# shellcheck source=/dev/null
|
||
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}"
|
||
FCU_URL="${DEV}:${BAUD}"
|
||
|
||
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 "${BRIDGE_PYTHON:-}" ]] && [[ -x "$BRIDGE_PYTHON" ]]; then
|
||
echo "$BRIDGE_PYTHON"
|
||
return
|
||
fi
|
||
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
|
||
}
|
||
|
||
echo "===== flight_intent 伴飞桥（含 MAVROS）fcu_url=${FCU_URL} ====="
|
||
|
||
if ! master_ok; then
|
||
echo "[1/3] 启动 roscore …"
|
||
roscore > /tmp/roscore_flight_bridge.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_flight_bridge.log" >&2
|
||
kill_children
|
||
exit 1
|
||
fi
|
||
echo "roscore 已就绪 (pid=$ROSCORE_PID)"
|
||
else
|
||
echo "[1/3] 已有 ROS master，跳过 roscore"
|
||
fi
|
||
|
||
echo "[2/3] 启动 MAVROS …"
|
||
stop_mavros
|
||
sleep 1
|
||
roslaunch mavros px4.launch fcu_url:="$FCU_URL" > /tmp/mavros_flight_bridge.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_flight_bridge.log" >&2
|
||
echo "可重试: bash $0 ${DEV} 57600" >&2
|
||
kill_children
|
||
exit 1
|
||
fi
|
||
echo "MAVROS 已连接飞控"
|
||
|
||
PY="$(resolve_python)"
|
||
export PYTHONPATH="${PYTHONPATH:-}:${ROOT}"
|
||
|
||
echo "[3/3] 启动伴飞桥（Python: $PY）…"
|
||
echo " Ctrl+C 退出并清理本脚本启动的 MAVROS$([[ "$WE_STARTED_ROSCORE" -eq 1 ]] && echo +roscore)。"
|
||
echo " 发意图: 另开终端 source noetic 后 rostopic pub ... 见脚本头注释。"
|
||
set +e
|
||
"$PY" -m voice_drone.flight_bridge.ros1_node
|
||
BRIDGE_EXIT=$?
|
||
set -e
|
||
|
||
kill_children
|
||
exit "$BRIDGE_EXIT"
|