ywb
178 lines
6.3 KiB
Bash
Executable File
178 lines
6.3 KiB
Bash
Executable File
#!/bin/bash
|
||
# ============================================================
|
||
# Robot AGV 全量启动脚本 v2.6
|
||
# 修复：
|
||
# - 清理 scan_fixer lock 文件防残留
|
||
# - Nav2 节点检测 grep -c 改为单行输出
|
||
# - nohup 启动 Nav2 用 bash -c 包裹（确保 source 环境）
|
||
# ============================================================
|
||
set -e
|
||
|
||
AGV_APP_DIR="/home/elephant/work/agv_app"
|
||
AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
|
||
ROS_DOMAIN_ID_VAL=1
|
||
|
||
echo "=========================================="
|
||
echo " Robot AGV 全量启动 v2.6"
|
||
echo "=========================================="
|
||
echo ""
|
||
|
||
# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
|
||
echo "[1/7] 清理旧进程..."
|
||
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
|
||
pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
|
||
pkill -f "agv_pro_node" 2>/dev/null || true
|
||
pkill -f "lslidar_driver_node" 2>/dev/null || true
|
||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||
pkill -f "python.*app.py" 2>/dev/null || true
|
||
sleep 4
|
||
|
||
# 清理 scan_fixer 锁文件（防残留 PID 导致启动失败）
|
||
rm -f /tmp/scan_fixer.lock
|
||
|
||
echo " 清理完成"
|
||
|
||
# ---------- 2. 重启 ros2 daemon ----------
|
||
echo "[2/7] 重启 ros2 daemon..."
|
||
pkill -f "ros2-daemon" 2>/dev/null || true
|
||
sleep 2
|
||
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
|
||
sleep 5
|
||
echo " ros2 daemon 已就绪"
|
||
|
||
# ---------- 3. 启动 bringup (含激光雷达) ----------
|
||
echo "[3/7] 启动 AGV Bringup..."
|
||
source /opt/ros/humble/setup.bash
|
||
cd "$AGV_ROS2_DIR"
|
||
source install/setup.bash
|
||
nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
|
||
port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
|
||
BRINGUP_PID=$!
|
||
echo " bringup PID: $BRINGUP_PID"
|
||
|
||
echo " 等待 bringup 就绪..."
|
||
for i in $(seq 1 20); do
|
||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
|
||
echo " ✅ bringup 已就绪"
|
||
break
|
||
fi
|
||
sleep 2
|
||
done
|
||
|
||
# ---------- 4. 启动激光时间戳修正节点 ----------
|
||
echo "[4/7] 启动激光时间戳修正节点..."
|
||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||
sleep 2
|
||
|
||
# 确保 /scan 存在
|
||
for i in $(seq 1 10); do
|
||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
|
||
echo " /scan 话题已上线"
|
||
break
|
||
fi
|
||
sleep 2
|
||
done
|
||
|
||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
|
||
> /tmp/scan_fixer.log 2>&1 &
|
||
FIXER_PID=$!
|
||
echo " fix_scan_timestamp PID: $FIXER_PID"
|
||
sleep 5
|
||
|
||
# 验证 fixer 进程和 scan_corrected
|
||
FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
|
||
if [ "$FIXER_COUNT" -gt 1 ]; then
|
||
echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
|
||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||
sleep 2
|
||
rm -f /tmp/scan_fixer.lock
|
||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
|
||
> /tmp/scan_fixer.log 2>&1 &
|
||
FIXER_PID=$!
|
||
sleep 3
|
||
fi
|
||
|
||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
|
||
echo " ✅ /scan_corrected 已上线"
|
||
else
|
||
echo " ⚠️ /scan_corrected 未上线，检查日志："
|
||
tail -5 /tmp/scan_fixer.log
|
||
fi
|
||
|
||
# ---------- 5. 启动 Nav2 ----------
|
||
echo "[5/7] 启动 Nav2 导航..."
|
||
source /opt/ros/humble/setup.bash
|
||
cd "$AGV_ROS2_DIR"
|
||
source install/setup.bash
|
||
# 使用 bash -c 确保 source 环境变量传递到 nohup
|
||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
||
source /home/elephant/agv_pro_ros2/install/setup.bash && \
|
||
export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && \
|
||
ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
|
||
autostart:=True" > /tmp/ros2_nav2.log 2>&1 &
|
||
NAV2_PID=$!
|
||
echo " Nav2 PID: $NAV2_PID"
|
||
sleep 12
|
||
|
||
echo " 等待 Nav2 节点就绪..."
|
||
for i in $(seq 1 20); do
|
||
NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
|
||
grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0)
|
||
# 去除可能的换行符，确保是单个数字
|
||
NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}')
|
||
if [ "$NODES" -ge 3 ] 2>/dev/null; then
|
||
echo " ✅ Nav2 节点已就绪 ($NODES 个)"
|
||
break
|
||
fi
|
||
sleep 3
|
||
done
|
||
|
||
# ---------- 6. 设置精度参数 ----------
|
||
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
|
||
source /opt/ros/humble/setup.bash
|
||
cd "$AGV_ROS2_DIR"
|
||
source install/setup.bash
|
||
|
||
for NODE in /controller_server /bt_navigator /planner_server; do
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
|
||
done
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
|
||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
|
||
echo " 精度参数已设置"
|
||
|
||
# ---------- 7. 启动 Flask ----------
|
||
echo "[7/7] 启动 Flask API..."
|
||
cd "$AGV_APP_DIR"
|
||
nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
|
||
FLASK_PID=$!
|
||
echo " Flask PID: $FLASK_PID"
|
||
sleep 4
|
||
|
||
# ---------- 完成 ----------
|
||
echo ""
|
||
echo "=========================================="
|
||
echo " ✅ 启动完成"
|
||
echo "=========================================="
|
||
echo ""
|
||
echo " 进程状态:"
|
||
for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
|
||
NAME="${PROC%%:*}"
|
||
PID="${PROC##*:}"
|
||
echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
|
||
done
|
||
echo ""
|
||
echo " 日志文件:"
|
||
echo " bringup : /tmp/ros2_bringup.log"
|
||
echo " Nav2 : /tmp/ros2_nav2.log"
|
||
echo " fixer : /tmp/scan_fixer.log"
|
||
echo " Flask : /tmp/agv_flask.log"
|
||
echo ""
|
||
echo " 关键验证命令:"
|
||
echo " curl http://localhost:5000/api/navigate/status"
|
||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
|
||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
|