导航修复
This commit is contained in:
ywb
+99
-77
@@ -1,71 +1,104 @@
|
||||
#!/bin/bash
|
||||
# ============================================================
|
||||
# Robot AGV 全量启动脚本
|
||||
# 完整流程：清理旧进程 -> 启动 bringup -> 启动 Nav2 -> 激活 Lifecycle -> 启动 Flask
|
||||
# Robot AGV 全量启动脚本 v2.2
|
||||
# 完整流程：
|
||||
# 清理旧进程（不杀 daemon） -> 启动 bringup ->
|
||||
# 启动激光时间戳修正节点 -> 启动 Nav2 ->
|
||||
# 设置导航精度参数 -> 启动 Flask
|
||||
# ============================================================
|
||||
set -e
|
||||
|
||||
LOG_DIR="/home/elephant/work/agv_app"
|
||||
cd "$LOG_DIR"
|
||||
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 全量启动"
|
||||
echo " Robot AGV 全量启动 v2.2"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
|
||||
# ---------- 1. 清理旧进程 ----------
|
||||
echo "[1/6] 清理旧进程..."
|
||||
# ---------- 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 "scan_timestamp_fixer" 2>/dev/null || true
|
||||
pkill -f "python.*app.py" 2>/dev/null || true
|
||||
pkill -f "ros2-daemon" 2>/dev/null || true
|
||||
sleep 4
|
||||
echo " 清理完成"
|
||||
|
||||
# ---------- 2. 重启 ros2 daemon ----------
|
||||
echo "[2/6] 重启 ros2 daemon..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
ros2 daemon stop 2>/dev/null || true
|
||||
# ---------- 2. 重启 ros2 daemon（仅杀 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 &
|
||||
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
|
||||
sleep 5
|
||||
|
||||
# 检查 daemon 是否正常
|
||||
if ! bash -c "source /opt/ros/humble/setup.bash && ros2 node list &>/dev/null"; then
|
||||
echo " ⚠️ 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 &
|
||||
sleep 5
|
||||
fi
|
||||
echo " ros2 daemon 已就绪"
|
||||
|
||||
# ---------- 3. 启动 bringup ----------
|
||||
echo "[3/6] 启动 AGV Bringup (agv_pro + 激光雷达)..."
|
||||
# ---------- 3. 启动 bringup (含激光雷达) ----------
|
||||
echo "[3/7] 启动 AGV Bringup..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd /home/elephant/agv_pro_ros2
|
||||
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"
|
||||
|
||||
# 等待 bringup 就绪（检查 /odom 话题出现）
|
||||
echo " 等待 bringup 就绪..."
|
||||
for i in $(seq 1 15); do
|
||||
if bash -c "source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 topic list 2>/dev/null | grep -q '/odom'"; then
|
||||
echo " ✅ bringup 已就绪 (/odom 话题已上线)"
|
||||
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. 启动 Nav2 ----------
|
||||
echo "[4/6] 启动 Nav2 导航..."
|
||||
# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ----------
|
||||
echo "[4/7] 启动激光时间戳修正节点..."
|
||||
# 确保只有1个 fixer 进程在运行
|
||||
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
|
||||
sleep 2
|
||||
|
||||
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 " scan_timestamp_fixer PID: $FIXER_PID"
|
||||
sleep 5
|
||||
|
||||
# 验证只有1个 fixer 进程
|
||||
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 "scan_timestamp_fixer" 2>/dev/null || true
|
||||
sleep 2
|
||||
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 &
|
||||
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 未上线，检查日志："
|
||||
cat /tmp/scan_fixer.log
|
||||
fi
|
||||
|
||||
# ---------- 5. 启动 Nav2 ----------
|
||||
echo "[5/7] 启动 Nav2 导航..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd /home/elephant/agv_pro_ros2
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
|
||||
autostart:=True > /tmp/ros2_nav2.log 2>&1 &
|
||||
@@ -73,44 +106,35 @@ NAV2_PID=$!
|
||||
echo " Nav2 PID: $NAV2_PID"
|
||||
sleep 12
|
||||
|
||||
# ---------- 5. 等待并激活 Nav2 Lifecycle ----------
|
||||
echo "[5/6] 等待 Nav2 Lifecycle Manager 就绪..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd /home/elephant/agv_pro_ros2
|
||||
source install/setup.bash
|
||||
|
||||
for i in $(seq 1 10); do
|
||||
NODES=$(ros2 node list 2>/dev/null | grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" || echo 0)
|
||||
echo " 检查 ($i/10): $NODES 个 Nav2 节点已启动"
|
||||
echo " 等待 Nav2 节点就绪..."
|
||||
for i in $(seq 1 15); do
|
||||
NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
|
||||
grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0)
|
||||
if [ "$NODES" -ge 3 ]; then
|
||||
echo " ✅ Nav2 节点已就绪"
|
||||
echo " ✅ Nav2 节点已就绪 ($NODES 个)"
|
||||
break
|
||||
fi
|
||||
sleep 3
|
||||
done
|
||||
|
||||
# 检查是否已 Active（autostart=True 应该自动激活）
|
||||
echo " 检查 Lifecycle 状态..."
|
||||
LIFECYCLE_STATE=$(ros2 lifecycle list /lifecycle_manager_navigation 2>/dev/null | grep "Active\|Inactive" | head -1 || echo "")
|
||||
if echo "$LIFECYCLE_STATE" | grep -q "Active"; then
|
||||
echo " ✅ Nav2 Lifecycle 已激活 (autostart=True 生效)"
|
||||
else
|
||||
echo " ⚠️ Lifecycle 未激活，手动 configure + activate..."
|
||||
ros2 lifecycle set /lifecycle_manager_navigation configure 2>/dev/null || true
|
||||
sleep 3
|
||||
ros2 lifecycle set /lifecycle_manager_navigation activate 2>/dev/null || true
|
||||
sleep 3
|
||||
LIFECYCLE_STATE=$(ros2 lifecycle list /lifecycle_manager_navigation 2>/dev/null | grep "Active\|Inactive" | head -1 || echo "")
|
||||
if echo "$LIFECYCLE_STATE" | grep -q "Active"; then
|
||||
echo " ✅ Lifecycle 手动激活成功"
|
||||
else
|
||||
echo " ⚠️ Lifecycle 仍无法激活，继续（Nav2 action 可能仍可用）"
|
||||
fi
|
||||
fi
|
||||
# ---------- 6. 设置精度参数 ----------
|
||||
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
|
||||
# ---------- 6. 启动 Flask ----------
|
||||
echo "[6/6] 启动 Flask API 服务..."
|
||||
cd /home/elephant/work/agv_app
|
||||
for NODE in /controller_server /bt_navigator /planner_server; do
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
|
||||
done
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL 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"
|
||||
@@ -123,21 +147,19 @@ echo " ✅ 启动完成"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
echo " 进程状态:"
|
||||
echo " bringup : $(ps aux | grep -w "$BRINGUP_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
|
||||
echo " Nav2 : $(ps aux | grep -w "$NAV2_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
|
||||
echo " Flask : $(ps aux | grep -w "$FLASK_PID" | grep -v grep | awk '{print $2}' || 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 " Flask : /tmp/agv_flask.log"
|
||||
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 " API 地址: http://localhost:5000"
|
||||
echo ""
|
||||
echo " 常用检查命令:"
|
||||
echo " curl http://localhost:5000/api/status"
|
||||
echo " 关键验证命令:"
|
||||
echo " curl http://localhost:5000/api/navigate/status"
|
||||
echo " curl -X POST http://localhost:5000/api/mission/init_pose"
|
||||
echo " curl -X POST http://localhost:5000/api/device/connect -H 'Content-Type: application/json' -d '{\"device\":\"agv\"}'"
|
||||
echo " curl -X POST http://localhost:5000/api/navigate/to -H 'Content-Type: application/json' -d '{\"x\":-0.249,\"y\":-0.957}'"
|
||||
echo ""
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
|
||||
Reference in New Issue
Block a user