FaulknerWu/smart-inspection
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

导航修复

Browse Source
This commit is contained in:
ywb
2026-05-19 15:59:23 +08:00
parent 50f6d4c295
commit a160c18ba7
4 changed files with 425 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/start_all.sh
+225 -53
View File
@@ -1,10 +1,11 @@
#!/bin/bash #!/bin/bash
# ============================================================ # ============================================================
# Robot AGV 全量启动脚本 v2.2 # Robot AGV 全量启动脚本 v4.0
# 完整流程： # 修复：
# 清理旧进程（不杀 daemon） -> 启动 bringup -> # - v4.0: 彻底杀死 ros2 daemon 进程 + 启动前进程数量检查
# 启动激光时间戳修正节点 -> 启动 Nav2 -> # - v3.0: 彻底清理 FastRTPS 共享内存文件（永久修复 DDS 通信问题）
# 设置导航精度参数 -> 启动 Flask # - v2.7: 添加 ROS_DOMAIN_ID 环境变量传递
# - v2.6: 清理 scan_fixer lock 文件防残留
# ============================================================ # ============================================================
set -e set -e
@@ -13,78 +14,180 @@ AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
ROS_DOMAIN_ID_VAL=1 ROS_DOMAIN_ID_VAL=1
echo "==========================================" echo "=========================================="
echo " Robot AGV 全量启动 v2.2" echo " Robot AGV 全量启动 v4.0"
echo "==========================================" echo "=========================================="
echo "" echo ""
# ---------- 1. 清理旧进程（不杀 ros2-daemon） ---------- # ---------- 1. 清理旧进程 + FastRTPS 共享内存 ----------
echo "[1/7] 清理旧进程..." echo "[1/8] 清理旧进程和共享内存..."
# 杀掉所有相关进程（先软杀，再硬杀确保干净）
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
pkill -f "ros2 launch agv_pro_navigation2" 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 "agv_pro_node" 2>/dev/null || true
pkill -f "lslidar_driver_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 "component_container" 2>/dev/null || true
pkill -f "robot_state_publisher" 2>/dev/null || true
pkill -f "fix_scan_timestamp" 2>/dev/null || true
pkill -f "clock_publisher" 2>/dev/null || true
pkill -f "python.*app.py" 2>/dev/null || true pkill -f "python.*app.py" 2>/dev/null || true
sleep 4
echo " 清理完成"
# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ----------
echo "[2/7] 重启 ros2 daemon..."
pkill -f "ros2-daemon" 2>/dev/null || true
sleep 2 sleep 2
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
sleep 5 # 【关键】硬杀确保干净
echo " ros2 daemon 已就绪" echo " 硬杀残留进程..."
pkill -9 -f "agv_pro_node" 2>/dev/null || true
pkill -9 -f "lslidar_driver_node" 2>/dev/null || true
pkill -9 -f "component_container" 2>/dev/null || true
pkill -9 -f "clock_publisher" 2>/dev/null || true
pkill -9 -f "fix_scan_timestamp" 2>/dev/null || true
pkill -9 -f "app.py" 2>/dev/null || true
sleep 1
# 【关键】杀死 ros2 daemon 进程本身（不是只 stop，而是杀进程）
echo " 重置 ros2 daemon..."
pkill -f "ros2-daemon" 2>/dev/null || true
pkill -9 -f "ros2-daemon" 2>/dev/null || true
sleep 2
# 【关键】清理 FastRTPS 共享内存文件（杀进程后立即清理）
echo " 清理 FastRTPS 共享内存文件..."
FASTRTPS_COUNT=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)
if [ "$FASTRTPS_COUNT" -gt 0 ]; then
rm -rf /dev/shm/fastrtps_*
echo " 已清理 $FASTRTPS_COUNT 个 FastRTPS 文件"
else
echo " 无 FastRTPS 文件残留"
fi
# 清理 scan_fixer 锁文件
rm -f /tmp/scan_fixer.lock
# 【关键】验证进程已全部停止
echo " 验证进程停止..."
PROC_COUNT=$(ps aux | grep -E 'agv_pro_node|lslidar_driver_node|component_container|fix_scan_timestamp|app.py' | grep -v grep | wc -l || echo 0)
echo " 残留进程数: $PROC_COUNT"
if [ "$PROC_COUNT" -gt 0 ]; then
echo " ⚠️ 仍有进程残留，强制终止..."
pkill -9 -f "agv_pro_node" 2>/dev/null || true
pkill -9 -f "lslidar_driver_node" 2>/dev/null || true
pkill -9 -f "component_container" 2>/dev/null || true
pkill -9 -f "fix_scan_timestamp" 2>/dev/null || true
pkill -9 -f "app.py" 2>/dev/null || true
sleep 2
PROC_COUNT2=$(ps aux | grep -E 'agv_pro_node|lslidar_driver_node|component_container|fix_scan_timestamp|app.py' | grep -v grep | wc -l || echo 0)
echo " 清理后残留: $PROC_COUNT2"
fi
echo " ✅ 清理完成"
# ---------- 2. 启动 ros2 daemon ----------
echo "[2/8] 启动 ros2 daemon..."
source /opt/ros/humble/setup.bash 2>/dev/null || true
# 再次确保没有残留共享内存（启动 daemon 前）
rm -rf /dev/shm/fastrtps_* 2>/dev/null || true
# 使用 bash -c 确保环境变量正确传递
nohup bash -c "source /opt/ros/humble/setup.bash && export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && ros2 daemon start" >/dev/null 2>&1 &
sleep 4
# 验证 daemon 是否就绪（用简单的 topic list 测试）
DAEMON_OK=0
for i in $(seq 1 5); do
DAEMON_TOPICS=$(source /opt/ros/humble/setup.bash && ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 3 ros2 topic list 2>&1 | wc -l || echo 0)
if [ "$DAEMON_TOPICS" -gt 0 ]; then
DAEMON_OK=1
echo " ✅ ros2 daemon 就绪"
break
fi
sleep 2
done
if [ "$DAEMON_OK" -eq 0 ]; then
echo " ⚠️ ros2 daemon 可能有问题，继续尝试启动组件..."
fi
# ---------- 3. 启动 bringup (含激光雷达) ---------- # ---------- 3. 启动 bringup (含激光雷达) ----------
echo "[3/7] 启动 AGV Bringup..." echo "[3/8] 启动 AGV Bringup..."
source /opt/ros/humble/setup.bash source /opt/ros/humble/setup.bash 2>/dev/null || true
# 【关键】启动前最后确认没有残留共享内存
rm -rf /dev/shm/fastrtps_* 2>/dev/null || true
cd "$AGV_ROS2_DIR" cd "$AGV_ROS2_DIR"
source install/setup.bash 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 & nohup bash -c "export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller" > /tmp/ros2_bringup.log 2>&1 &
BRINGUP_PID=$! BRINGUP_PID=$!
echo " bringup PID: $BRINGUP_PID" echo " bringup PID: $BRINGUP_PID"
echo " 等待 bringup 就绪..." echo " 等待 bringup 就绪..."
BRINGUP_OK=0
for i in $(seq 1 20); do 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 if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
echo " ✅ bringup 已就绪" echo " ✅ bringup 已就绪 (${i}x2秒)"
BRINGUP_OK=1
break break
fi fi
sleep 2 sleep 2
done done
if [ "$BRINGUP_OK" -eq 0 ]; then
echo " ⚠️ bringup 未检测到 /odom，继续启动后续组件..."
tail -5 /tmp/ros2_bringup.log 2>/dev/null || true
fi
# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ---------- # ---------- 3.5 启动系统时钟发布器 ----------
echo "[4/7] 启动激光时间戳修正节点..." echo "[3.5/8] 启动系统时钟发布器 (clock_publisher)..."
# 确保只有1个 fixer 进程在运行
pkill -f "scan_timestamp_fixer" 2>/dev/null || true nohup bash -c "source /opt/ros/humble/setup.bash && \
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/clock_publisher.py" \
> /tmp/clock_publisher.log 2>&1 &
CLOCK_PID=$!
echo " clock_publisher PID: $CLOCK_PID"
sleep 2 sleep 2
# 验证 /clock 话题
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/clock'; then
echo " ✅ /clock 已上线"
else
echo " ⚠️ /clock 未上线，检查日志："
tail -5 /tmp/clock_publisher.log 2>/dev/null || true
fi
# ---------- 4. 启动激光时间戳修正节点 ----------
echo "[4/8] 启动激光时间戳修正节点..."
# 确保 /scan 存在
SCAN_OK=0
for i in $(seq 1 10); do 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 if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
echo " /scan 话题已上线" echo " /scan 话题已上线"
SCAN_OK=1
break break
fi fi
sleep 2 sleep 2
done done
if [ "$SCAN_OK" -eq 0 ]; then
echo " ⚠️ /scan 未上线，检查 bringup 日志"
fi
nohup bash -c "source /opt/ros/humble/setup.bash && \ 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" \ ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp_v6.py" \
> /tmp/scan_fixer.log 2>&1 & > /tmp/scan_fixer.log 2>&1 &
FIXER_PID=$! FIXER_PID=$!
echo " scan_timestamp_fixer PID: $FIXER_PID" echo " fix_scan_timestamp PID: $FIXER_PID"
sleep 5 sleep 5
# 验证只有1个 fixer 进程 # 验证 fixer 进程和 scan_corrected
FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0) FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
if [ "$FIXER_COUNT" -gt 1 ]; then if [ "$FIXER_COUNT" -gt 1 ]; then
echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..." echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
pkill -f "scan_timestamp_fixer" 2>/dev/null || true pkill -f "fix_scan_timestamp" 2>/dev/null || true
pkill -f "clock_publisher" 2>/dev/null || true
sleep 2 sleep 2
rm -f /tmp/scan_fixer.lock
nohup bash -c "source /opt/ros/humble/setup.bash && \ 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" \ ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp_v6.py" \
> /tmp/scan_fixer.log 2>&1 & > /tmp/scan_fixer.log 2>&1 &
FIXER_PID=$!
sleep 3 sleep 3
fi fi
@@ -92,54 +195,122 @@ if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan
echo " ✅ /scan_corrected 已上线" echo " ✅ /scan_corrected 已上线"
else else
echo " ⚠️ /scan_corrected 未上线，检查日志：" echo " ⚠️ /scan_corrected 未上线，检查日志："
cat /tmp/scan_fixer.log tail -5 /tmp/scan_fixer.log 2>/dev/null || true
fi fi
# ---------- 5. 启动 Nav2 ---------- # ---------- 5. 启动 Nav2 ----------
echo "[5/7] 启动 Nav2 导航..." echo "[5/8] 启动 Nav2 导航..."
source /opt/ros/humble/setup.bash source /opt/ros/humble/setup.bash 2>/dev/null || true
cd "$AGV_ROS2_DIR" cd "$AGV_ROS2_DIR"
source install/setup.bash source install/setup.bash
nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
autostart:=True > /tmp/ros2_nav2.log 2>&1 & 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=$! NAV2_PID=$!
echo " Nav2 PID: $NAV2_PID" echo " Nav2 PID: $NAV2_PID"
sleep 12 sleep 12
echo " 等待 Nav2 节点就绪..." echo " 等待 Nav2 节点就绪..."
NAV2_OK=0
for i in $(seq 1 15); do for i in $(seq 1 15); do
NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \ 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) grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0)
if [ "$NODES" -ge 3 ]; then NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}')
if [ "$NODES" -ge 3 ] 2>/dev/null; then
echo " ✅ Nav2 节点已就绪 ($NODES 个)" echo " ✅ Nav2 节点已就绪 ($NODES 个)"
NAV2_OK=1
break break
fi fi
sleep 3 sleep 3
done done
if [ "$NAV2_OK" -eq 0 ]; then
echo " ⚠️ Nav2 节点未完全就绪，继续..."
fi
# ---------- 6. 设置精度参数 ---------- # ---------- 6. 设置精度参数 ----------
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..." echo "[6/8] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
source /opt/ros/humble/setup.bash source /opt/ros/humble/setup.bash 2>/dev/null || true
cd "$AGV_ROS2_DIR" cd "$AGV_ROS2_DIR"
source install/setup.bash source install/setup.bash
for NODE in /controller_server /bt_navigator /planner_server; do 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 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 ros2 param set $NODE general_goal_checker.yaw_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 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 timeout 1 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 timeout 1 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 ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
echo " 精度参数已设置" echo " ✅ 精度参数已设置"
# ---------- 7. 启动 Flask ---------- # ---------- 7. 启动 Flask ----------
echo "[7/7] 启动 Flask API..." echo "[7/8] 启动 Flask API..."
cd "$AGV_APP_DIR" cd "$AGV_APP_DIR"
nohup python3 app.py > /tmp/agv_flask.log 2>&1 & nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
FLASK_PID=$! FLASK_PID=$!
echo " Flask PID: $FLASK_PID" echo " Flask PID: $FLASK_PID"
sleep 4 sleep 4
# ---------- 8. 最终全面验证 ----------
echo ""
echo "=========================================="
echo " 系统全面验证"
echo "=========================================="
# 8a. 验证 ros2 topic list（核心指标）
echo ""
echo "验证 ros2 topic list..."
TOPIC_COUNT=$(source /opt/ros/humble/setup.bash && ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 5 ros2 topic list 2>/dev/null | wc -l || echo 0)
echo " 话题数量: $TOPIC_COUNT"
if [ "$TOPIC_COUNT" -gt 10 ]; then
echo " ✅ ros2 daemon 正常 (${TOPIC_COUNT} 个话题)"
else
echo " ❌ ros2 topic list 异常 (${TOPIC_COUNT} 个话题，可能 DDS 有问题)"
echo " 手动执行: rm -rf /dev/shm/fastrtps_* && ros2 daemon stop && ros2 daemon start"
fi
# 8b. 验证关键话题
echo ""
echo "验证关键话题..."
for TOPIC in /odom /scan /cmd_vel /tf; do
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q "$TOPIC"; then
echo " ✅ $TOPIC"
else
echo " ⚠️ $TOPIC 未找到"
fi
done
# 8c. 验证进程数量（确保没有重复启动）
echo ""
echo "验证进程数量..."
BRINGUP_PROCS=$(ps aux | grep -E 'agv_pro_node|lslidar_driver_node' | grep -v grep | wc -l || echo 0)
echo " AGV 核心进程: $BRINGUP_PROCS (应为 2)"
if [ "$BRINGUP_PROCS" -eq 2 ]; then
echo " ✅ 进程数量正常（无重复）"
elif [ "$BRINGUP_PROCS" -gt 2 ]; then
echo " ⚠️ 发现 $BRINGUP_PROCS 个核心进程（可能有残留），建议重启"
else
echo " ⚠️ 进程数量异常"
fi
# 8d. FastRTPS 共享内存状态
echo ""
echo "FastRTPS 共享内存状态:"
FASTRTPS_NEW=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)
echo " 当前文件数: $FASTRTPS_NEW (正常运行时会有一些)"
# 8e. Flask API 测试
echo ""
echo "验证 Flask API..."
FLASK_RUNNING=$(ps aux | grep "[p]ython3 app.py" | wc -l || echo 0)
if [ "$FLASK_RUNNING" -gt 0 ]; then
echo " ✅ Flask 进程运行中"
else
echo " ❌ Flask 未运行"
fi
# ---------- 完成 ---------- # ---------- 完成 ----------
echo "" echo ""
echo "==========================================" echo "=========================================="
@@ -150,7 +321,8 @@ echo " 进程状态:"
for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
NAME="${PROC%%:*}" NAME="${PROC%%:*}"
PID="${PROC##*:}" PID="${PROC##*:}"
echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')" STATUS=$(ps aux | grep -w "$PID" | grep -v grep | awk '{print "运行中"}' || echo '已退出')
echo " $NAME : $STATUS"
done done
echo "" echo ""
echo " 日志文件:" echo " 日志文件:"
@@ -159,7 +331,7 @@ echo " Nav2 : /tmp/ros2_nav2.log"
echo " fixer : /tmp/scan_fixer.log" echo " fixer : /tmp/scan_fixer.log"
echo " Flask : /tmp/agv_flask.log" echo " Flask : /tmp/agv_flask.log"
echo "" echo ""
echo " 关键验证命令:" echo " 如果仍有问题，请依次执行："
echo " curl http://localhost:5000/api/navigate/status" echo " 1. ./stop_all.sh"
echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once" echo " 2. rm -rf /dev/shm/fastrtps_*"
echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once" echo " 3. ./start_all.sh"
agv_app/stop_all.sh
+80 -29
View File
@@ -1,38 +1,89 @@
#!/bin/bash #!/bin/bash
# ============================================================
# stop_all.sh - 关闭 AGV 拍摄系统所有相关进程 # stop_all.sh - 关闭 AGV 拍摄系统所有相关进程
# 版本: v2.0
# 修复:
# - v2.0: 添加 FastRTPS 清理 + ros2 daemon 重置
# ============================================================
set -e
echo "正在停止 AGV 系统..." echo "=========================================="
echo " Robot AGV 全量停止"
echo "=========================================="
echo ""
# 1. 关闭 Flask 主程序 # ---------- 1. 软杀所有相关进程 ----------
echo "[1/3] 关闭 Flask 服务..." echo "[1/5] 软杀所有相关进程..."
pkill -f "python3 app.py" 2>/dev/null pkill -f "python3 app.py" 2>/dev/null || true
pkill -f "agv_pro_bringup" 2>/dev/null || true
pkill -f "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 "component_container" 2>/dev/null || true
pkill -f "fix_scan_timestamp" 2>/dev/null || true
pkill -f "robot_state_publisher" 2>/dev/null || true
pkill -f "start_all.sh" 2>/dev/null || true
sleep 2
# ---------- 2. 硬杀确保干净 ----------
echo "[2/5] 硬杀残留进程..."
pkill -9 -f "app.py" 2>/dev/null || true
pkill -9 -f "agv_pro_node" 2>/dev/null || true
pkill -9 -f "lslidar_driver_node" 2>/dev/null || true
pkill -9 -f "component_container" 2>/dev/null || true
pkill -9 -f "fix_scan_timestamp" 2>/dev/null || true
pkill -9 -f "agv_pro_bringup" 2>/dev/null || true
pkill -9 -f "agv_pro_navigation2" 2>/dev/null || true
sleep 1 sleep 1
# 2. 关闭 ROS2 bringup（杀掉整个 start.sh 及其子进程） # ---------- 3. 【关键】清理 FastRTPS 共享内存 ----------
echo "[2/3] 关闭 ROS2 bringup..." echo "[3/5] 清理 FastRTPS 共享内存..."
# 用 pkill 杀掉 agv_pro_bringup 相关进程 FASTRTPS_COUNT=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)
pkill -f "agv_pro_bringup" 2>/dev/null if [ "$FASTRTPS_COUNT" -gt 0 ]; then
pkill -f "agv_pro_navigation2" 2>/dev/null rm -rf /dev/shm/fastrtps_*
# 如果 bringup 是由 start.sh 启动的，杀掉 start.sh 进程树 echo " 已清理 $FASTRTPS_COUNT 个 FastRTPS 文件"
pkill -f "start.sh" 2>/dev/null
pkill -f "bringup" 2>/dev/null
sleep 1
# 3. 清理 ROS2 daemon（防止残留）
echo "[3/3] 清理 ROS2 daemon..."
source /opt/ros/humble/setup.bash 2>/dev/null
ros2 daemon stop 2>/dev/null || true
# 确保所有相关进程都已停止
sleep 1
remaining=$(ps aux | grep -E "python3 app.py|agv_pro_bringup|agv_pro_navigation2|navigation2" | grep -v grep | wc -l)
if [ "$remaining" -eq 0 ]; then
echo "✅ 所有进程已关闭"
else else
echo "⚠️ 仍有 $remaining 个进程残留，已强制终止" echo " 无 FastRTPS 文件残留"
pkill -9 -f "python3 app.py" 2>/dev/null
pkill -9 -f "agv_pro_bringup" 2>/dev/null
pkill -9 -f "agv_pro_navigation2" 2>/dev/null
fi fi
echo "停止完成" # 清理 scan_fixer 锁文件
rm -f /tmp/scan_fixer.lock
echo " ✅ FastRTPS 清理完成"
# ---------- 4. 【关键】重置 ros2 daemon ----------
echo "[4/5] 重置 ros2 daemon..."
pkill -f "ros2-daemon" 2>/dev/null || true
pkill -9 -f "ros2-daemon" 2>/dev/null || true
sleep 2
source /opt/ros/humble/setup.bash 2>/dev/null || true
ros2 daemon stop 2>/dev/null || true
echo " ✅ ros2 daemon 已重置"
# ---------- 5. 验证清理结果 ----------
echo "[5/5] 验证清理结果..."
PROC_COUNT=$(ps aux | grep -E 'agv_pro_node|lslidar_driver_node|component_container|fix_scan_timestamp|app.py|ros2-daemon' | grep -v grep | wc -l || echo 0)
FASTRTPS_LEFT=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)
echo " 残留进程数: $PROC_COUNT"
echo " FastRTPS 文件数: $FASTRTPS_LEFT"
if [ "$PROC_COUNT" -eq 0 ] && [ "$FASTRTPS_LEFT" -eq 0 ]; then
echo ""
echo "=========================================="
echo " ✅ 停止完成 - 系统已完全清理"
echo "=========================================="
else
echo ""
echo "=========================================="
echo " ⚠️ 停止完成 - 部分残留可能需要手动清理"
echo "=========================================="
echo ""
echo " 手动清理命令（如需要）:"
echo " pkill -9 -f 'agv_pro_node|lslidar|component_container'"
echo " pkill -9 -f 'fix_scan_timestamp|app.py'"
echo " pkill -9 -f 'ros2-daemon'"
echo " rm -rf /dev/shm/fastrtps_*"
fi
echo ""
echo " 现在可以安全运行 ./start_all.sh"
echo ""
agv_app/utils/agv_controller_ros2.py
+14 -32
View File
@@ -44,41 +44,23 @@ class AGVController:
return -1, "", str(e) return -1, "", str(e)
def connect(self) -> bool: def connect(self) -> bool:
"""连接 AGV - 检查 ROS2 节点和 topic""" """连接 AGV - 检查 ROS2 topic 是否可用"""
try: try:
# 检查 agv_pro_node 是否运行 # 检查 /odom topic 是否存在（比 ros2 node list 更可靠）
rc, out, err = self._run_ros2_cmd("ros2 node list")
if rc != 0:
logger.error(f"ROS2 节点列表获取失败: {err}")
return False
if "/agv_pro_node" not in out:
logger.error("agv_pro_node 未运行")
return False
# 检查 /odom topic
rc, out, err = self._run_ros2_cmd("ros2 topic list") rc, out, err = self._run_ros2_cmd("ros2 topic list")
if rc != 0:
logger.error(f"ROS2 topic 列表获取失败: {err}")
return False
if "/odom" not in out: if "/odom" not in out:
logger.error("/odom topic 不存在") logger.error("/odom topic 不存在")
return False return False
# 尝试获取一次位置数据 # /odom 存在即表示 AGV 节点在运行
rc, out, err = self._run_ros2_cmd( self._connected = True
"timeout 5 ros2 topic echo /odom --once 2>/dev/null", self._ros2_available = True
timeout=6 logger.info("AGV ROS2 连接成功 (/odom topic 存在)")
) return True
if rc == 0 and out:
self._connected = True
self._ros2_available = True
logger.info("AGV ROS2 连接成功")
return True
else:
# /odom 可能暂时没数据，但节点存在也算连接成功
self._connected = True
self._ros2_available = True
logger.info("AGV ROS2 连接成功 (节点存在，等待 odom 数据)")
return True
except Exception as e: except Exception as e:
logger.error(f"AGV 连接失败: {e}") logger.error(f"AGV 连接失败: {e}")
@@ -154,13 +136,13 @@ class AGVController:
self._publish_cmd_vel(0, 0, 0) self._publish_cmd_vel(0, 0, 0)
def get_position(self) -> Optional[List[float]]: def get_position(self) -> Optional[List[float]]:
"""获取 AGV 当前位置 [x, y, yaw]""" """获取 AGV 当前位置 [x, y, yaw]（使用 AMCL 全局定位，比 /odom 更准确）"""
if not self.is_connected(): if not self.is_connected():
return None return None
try: try:
# 从 /odom topic 获取位置 # 优先从 /amcl_pose 获取全局定位位置（更准确）
rc, out, err = self._run_ros2_cmd( rc, out, err = self._run_ros2_cmd(
"timeout 5 ros2 topic echo /odom --once 2>/dev/null", "timeout 5 ros2 topic echo /amcl_pose --once 2>/dev/null",
timeout=6 timeout=6
) )
if rc == 0 and out: if rc == 0 and out:
agv_app/utils/nav2_navigator.py
+105 -247
View File
@@ -2,7 +2,7 @@
Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航 Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航
使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action 使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action
通过写入临时 shell 脚本再执行，避免 bash -c 单引号转义问题 通过 stdin pipe 发送 goal YAML，避免 bash $() 替换破坏 YAML 格式
""" """
import time import time
import math import math
@@ -10,6 +10,7 @@ import logging
import threading import threading
import subprocess import subprocess
import os import os
import re
from typing import List, Tuple, Optional, Dict from typing import List, Tuple, Optional, Dict
from enum import Enum from enum import Enum
@@ -39,55 +40,12 @@ class Nav2Status(Enum):
class Nav2Navigator: class Nav2Navigator:
"""Nav2 导航器 — 通过 ros2 action /navigate_to_pose 与 Nav2 通信""" """Nav2 导航器 — 通过 ros2 action /navigate_to_pose 与 Nav2 通信"""
NAVIGATE_SCRIPT = "/tmp/nav2_send_goal.sh"
def __init__(self): def __init__(self):
self.status = Nav2Status.IDLE self.status = Nav2Status.IDLE
self._nav_thread: Optional[threading.Thread] = None self._nav_thread: Optional[threading.Thread] = None
self._cancel_event = threading.Event() self._cancel_event = threading.Event()
self._result_status = None self._result_status = None
self._current_pose = [0.0, 0.0, 0.0] self._current_pose = [0.0, 0.0, 0.0]
self._build_script()
def _build_script(self):
"""生成 Nav2 action 发送脚本（通过 heredoc 避免所有 bash 转义问题）"""
script = self.NAVIGATE_SCRIPT
content = [
"#!/bin/bash",
"source /opt/ros/humble/setup.bash",
"source /home/elephant/agv_pro_ros2/install/setup.bash",
"",
"TARGET_X=${1:-0}",
"TARGET_Y=${2:-0}",
"TARGET_YAW=${3:-0}",
"",
"# 计算四元数",
"YAW_QZ=$(python3 -c \"import math; print(math.sin($TARGET_YAW / 2.0))\")",
"YAW_QW=$(python3 -c \"import math; print(math.cos($TARGET_YAW / 2.0))\")",
"",
"# 通过 stdin pipe 发送 goal",
'cat << \'GOAL\' | ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose - --feedback',
"pose:",
" header:",
" stamp:",
" sec: 0",
" nanosec: 0",
" frame_id: map",
" pose:",
" position:",
" x: $TARGET_X",
" y: $TARGET_Y",
" z: 0.0",
" orientation:",
" x: 0.0",
" y: 0.0",
" z: $YAW_QZ",
" w: $YAW_QW",
"GOAL",
]
with open(script, "w") as f:
f.write("\n".join(content) + "\n")
os.chmod(script, 0o755)
def _check_nav2_available(self) -> bool: def _check_nav2_available(self) -> bool:
"""检查 Nav2 action server 是否可用（带缓存 10 秒）""" """检查 Nav2 action server 是否可用（带缓存 10 秒）"""
@@ -95,7 +53,6 @@ class Nav2Navigator:
if hasattr(self, '_nav2_check_time') and hasattr(self, '_nav2_check_result'): if hasattr(self, '_nav2_check_time') and hasattr(self, '_nav2_check_result'):
if time_mod.time() - self._nav2_check_time < 10: if time_mod.time() - self._nav2_check_time < 10:
return self._nav2_check_result return self._nav2_check_result
# 写入脚本文件执行，兼容 Flask subprocess 环境
script = ( script = (
"#!/bin/bash\n" "#!/bin/bash\n"
"source /opt/ros/humble/setup.bash 2>/dev/null\n" "source /opt/ros/humble/setup.bash 2>/dev/null\n"
@@ -106,24 +63,20 @@ class Nav2Navigator:
script_file = "/tmp/nav2_check.sh" script_file = "/tmp/nav2_check.sh"
with open(script_file, "w") as f: with open(script_file, "w") as f:
f.write(script) f.write(script)
import os
os.chmod(script_file, 0o755) os.chmod(script_file, 0o755)
r = subprocess.run([script_file], capture_output=True, text=True, timeout=8) r = subprocess.run([script_file], capture_output=True, text=True, timeout=8)
rc = r.returncode result = r.returncode == 0 and ("/navigate_to_pose" in r.stdout or "navigate_to_pose" in r.stdout)
out = r.stdout logger.debug(f"_check_nav2_available: rc={r.returncode}, result={result}")
err = r.stderr
result = rc == 0 and ("/navigate_to_pose" in out or "navigate_to_pose" in out)
logger.debug(f"_check_nav2_available: rc={rc}, out={out[:100]}, result={result}")
self._nav2_check_result = result self._nav2_check_result = result
self._nav2_check_time = time_mod.time() self._nav2_check_time = time_mod.time()
return result return result
def _get_current_pose(self) -> List[float]: def _get_current_pose(self) -> List[float]:
"""从 /odom 获取当前位置 [x, y, yaw]""" """从 /amcl_pose 获取当前位置 [x, y, yaw]"""
try: try:
rc, out, err = _run_ros2_bash("timeout 3 ros2 topic echo /amcl_pose --once 2>/dev/null", timeout=4) rc, out, err = _run_ros2_bash("timeout 3 ros2 topic echo /amcl_pose --once 2>/dev/null", timeout=4)
if rc == 0 and out: if rc == 0 and out:
import re, yaml import yaml
clean = re.sub(r'\x1b\[[0-9;]*[a-zA-Z]', '', out) clean = re.sub(r'\x1b\[[0-9;]*[a-zA-Z]', '', out)
yaml_str = re.sub(r'^[\d\-:. ]+\[RTPS[^\]]*\].*\n?', '', clean, flags=re.MULTILINE).strip() yaml_str = re.sub(r'^[\d\-:. ]+\[RTPS[^\]]*\].*\n?', '', clean, flags=re.MULTILINE).strip()
yaml_str = yaml_str.split('---')[0] yaml_str = yaml_str.split('---')[0]
@@ -139,35 +92,22 @@ class Nav2Navigator:
self._current_pose = [x, y, yaw] self._current_pose = [x, y, yaw]
return self._current_pose return self._current_pose
except Exception as e: except Exception as e:
logger.debug(f"获取 odom 失败: {e}") logger.debug(f"获取 amcl_pose 失败: {e}")
return self._current_pose return self._current_pose
def navigate_to_pose(self, x: float, y: float, yaw: float = None, def navigate_to_pose(self, x: float, y: float, yaw: float = None,
timeout_sec: float = 120.0, timeout_sec: float = 120.0,
blocking: bool = True) -> bool: blocking: bool = True) -> bool:
""" """导航到目标坐标"""
导航到目标坐标（Nav2 navigate_to_pose action）
Args:
x, y: 目标位置（世界坐标，米）
yaw: 目标朝向（弧度），None 则保持当前朝向
timeout_sec: 超时时间（秒）
blocking: 是否阻塞等待导航完成
Returns:
blocking=True: 是否成功到达
blocking=False: 是否成功启动导航
"""
if self.status == Nav2Status.NAVIGATING: if self.status == Nav2Status.NAVIGATING:
logger.warning("导航正在进行中，请先停止当前导航") logger.warning("导航正在进行中，请先停止当前导航")
return False return False
# 重置状态，允许发起新导航
self.status = Nav2Status.IDLE self.status = Nav2Status.IDLE
self._result_status = None self._result_status = None
if not self._check_nav2_available(): if not self._check_nav2_available():
logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动") logger.error("Nav2 action server 不可用")
return False return False
if yaw is None: if yaw is None:
@@ -176,12 +116,10 @@ class Nav2Navigator:
logger.info(f"发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°") logger.info(f"发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
# 启动导航线程
self._cancel_event.clear() self._cancel_event.clear()
self._result_status = None self._result_status = None
self.status = Nav2Status.NAVIGATING self.status = Nav2Status.NAVIGATING
# 停掉旧线程（防止重复调用导致多线程冲突）
if self._nav_thread and self._nav_thread.is_alive(): if self._nav_thread and self._nav_thread.is_alive():
self.stop() self.stop()
self._nav_thread.join(timeout=3) self._nav_thread.join(timeout=3)
@@ -199,24 +137,32 @@ class Nav2Navigator:
return True return True
def _parse_result_status(self, combined: str) -> Optional[str]:
"""从 ros2 action send_goal 输出中解析导航结果状态"""
result_idx = combined.find("Result:")
if result_idx < 0:
return None
# 匹配 "status: STATUS_SUCCEEDED" 或 "status: SUCCEEDED" 等格式
status_m = re.search(r'status\s*:\s*(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).upper()
if 'SUCCEEDED' in st:
return "succeeded"
elif 'FAILED' in st or 'ABORTED' in st:
return "failed"
elif 'CANCELED' in st or 'CANCELLED' in st:
return "cancelled"
logger.warning(f"未知的导航状态: {st}")
return None
def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float): def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float):
"""导航执行线程 — 使用 heredoc 脚本避免 bash 转义问题""" """导航执行线程 — 使用 stdin pipe 发送 goal YAML"""
try: try:
logger.info(f"Nav2 线程启动: x={x}, y={y}, yaw={yaw}, timeout={timeout_sec}s") logger.info(f"Nav2 线程启动: x={x}, y={y}, yaw={yaw}, timeout={timeout_sec}s")
start_time = time.time() start_time = time.time()
elapsed = 0.0
check_interval = 1.0
result_received = False
# 使用 pipe 方式运行导航
# 通过 subprocess.Popen + stdin heredoc 避免所有引号转义问题
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
qz = math.sin(yaw / 2.0) qz = math.sin(yaw / 2.0)
qw = math.cos(yaw / 2.0) qw = math.cos(yaw / 2.0)
# 构建 goal YAML（用 bash heredoc 方式避免转义问题）
# 写入临时文件
import tempfile
goal_yaml = ( goal_yaml = (
f"pose:\n" f"pose:\n"
f" header:\n" f" header:\n"
@@ -234,23 +180,27 @@ class Nav2Navigator:
f" y: 0.0\n" f" y: 0.0\n"
f" z: {qz}\n" f" z: {qz}\n"
f" w: {qw}\n" f" w: {qw}\n"
f"behavior_tree: ''\n"
) )
goal_file = "/tmp/nav2_goal_{}.yaml".format(os.getpid()) # 写入临时文件，用 $(cat file) 方式传递（stdin pipe "-" 在 Humble 有 bug）
import tempfile
goal_file = "/tmp/nav2_goal_{}_{}.yaml".format(os.getpid(), int(time.time()))
with open(goal_file, "w") as f: with open(goal_file, "w") as f:
f.write(goal_yaml) f.write(goal_yaml)
cmd = ( # 写一个临时脚本执行导航（避免引号嵌套地狱）
f'bash -l -c \'' script_file = "/tmp/nav2_navigate_{}_{}.sh".format(os.getpid(), int(time.time()))
f'source /opt/ros/humble/setup.bash && ' with open(script_file, "w") as f:
f'source /home/elephant/agv_pro_ros2/install/setup.bash && ' f.write('#!/bin/bash\n')
f'ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose ' f.write('source /opt/ros/humble/setup.bash\n')
f'"$(cat {goal_file})" --feedback\'' f.write('source /home/elephant/agv_pro_ros2/install/setup.bash\n')
) f.write('export ROS_DOMAIN_ID=1\n')
f.write('ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "$(cat {})" --feedback\n'.format(goal_file))
os.chmod(script_file, 0o755)
process = subprocess.Popen( proc = subprocess.Popen(
cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True ['bash', '-l', script_file],
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
) )
stdout_lines = [] stdout_lines = []
@@ -263,155 +213,72 @@ class Nav2Navigator:
break break
lines.append(line) lines.append(line)
# 写入 goal（通过 bash heredoc） t_out = threading.Thread(target=reader, args=(proc.stdout, stdout_lines))
# 写入 goal（通过 bash 脚本方式） t_err = threading.Thread(target=reader, args=(proc.stderr, stderr_lines))
script_content = ( t_out.start()
f'#!/bin/bash\n' t_err.start()
f'source /opt/ros/humble/setup.bash\n'
f'source /home/elephant/agv_pro_ros2/install/setup.bash\n'
f'GOAL=$(cat {goal_file})\n'
f'ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "$GOAL" --feedback\n'
)
script_file = "/tmp/nav2_action.sh"
with open(script_file, "w") as sf:
sf.write(script_content)
os.chmod(script_file, 0o755)
out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines)) # 轮询等待 action 完成
err_thread = threading.Thread(target=reader, args=(process.stderr, stderr_lines))
out_thread.start()
err_thread.start()
# 用 subprocess.Popen([script_path]) 替代 stdin
stop_reading.set()
if process.poll() is None:
process.terminate()
process.wait(timeout=3)
act_proc = subprocess.Popen(
[script_file],
stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
)
stdout_lines2 = []
stderr_lines2 = []
stop_reading2 = threading.Event()
def reader2(pipe, lines):
for line in iter(pipe.readline, ''):
if stop_reading2.is_set():
break
lines.append(line)
t_out2 = threading.Thread(target=reader2, args=(act_proc.stdout, stdout_lines2))
t_err2 = threading.Thread(target=reader2, args=(act_proc.stderr, stderr_lines2))
t_out2.start()
t_err2.start()
# 轮询等待 action 完成（每 2s 检查一次结果）
result_received = False result_received = False
for _ in range(60): # 最多等 120 秒 max_checks = int(timeout_sec / 2)
for _ in range(max_checks):
time.sleep(2) time.sleep(2)
elapsed = time.time() - start_time elapsed = time.time() - start_time
# 检查是否有结果
full_out = "".join(stdout_lines2)
full_err = "".join(stderr_lines2)
combined = full_out + full_err
result_idx = combined.find("Result:")
if result_idx >= 0:
import re
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).lower()
if st in ('succeeded', 'SUCCEEDED'):
logger.info("✅ Nav2 导航成功到达目标")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 导航失败: status={st}")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
logger.info("Nav2 导航被取消")
self._result_status = "cancelled"
result_received = True
break
# 检查进程是否已结束但无结果
if act_proc.poll() is not None and not result_received:
self._result_status = "failed"
break
if self._cancel_event.is_set():
break
stop_reading2.set()
if act_proc.poll() is None:
act_proc.terminate()
try:
act_proc.wait(timeout=3)
except subprocess.TimeoutExpired:
act_proc.kill()
t_out2.join(timeout=2)
t_err2.join(timeout=2)
stdout_lines = stdout_lines2
stderr_lines = stderr_lines2
# 轮询检查结果
while not self._cancel_event.is_set() and elapsed < timeout_sec:
time.sleep(check_interval)
elapsed = time.time() - start_time
full_out = "".join(stdout_lines) full_out = "".join(stdout_lines)
full_err = "".join(stderr_lines) full_err = "".join(stderr_lines)
combined = full_out + full_err combined = full_out + full_err
# 查找 Result 节中的 status 字段 # 检查是否有 YAML 解析错误
import re if "Traceback" in combined and "Goal accepted" not in combined:
result_idx = combined.find("Result:") logger.error(f"ros2 action send_goal YAML 解析失败: {combined[:500]}")
if result_idx >= 0:
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).lower()
if st in ('succeeded', 'SUCCEEDED'):
logger.info("✅ Nav2 导航成功到达目标")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 导航失败: status={st}")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
logger.info("Nav2 导航被取消")
self._result_status = "cancelled"
result_received = True
break
if not result_received:
self._result_status = "failed" self._result_status = "failed"
result_received = True
break break
# 善后 # 检查是否未被接受
if "Goal accepted" not in combined and elapsed > 15:
logger.warning(f"导航目标 15 秒内未被接受，输出: {combined[:300]}")
# 解析 Result
parsed = self._parse_result_status(combined)
if parsed:
self._result_status = parsed
result_received = True
if parsed == "succeeded":
logger.info(f"✅ Nav2 导航成功到达目标 ({x:.3f}, {y:.3f})")
elif parsed == "failed":
logger.warning(f"⚠️ Nav2 导航失败")
else:
logger.info("Nav2 导航被取消")
break
# 进程已结束但无 Result
if proc.poll() is not None and not result_received:
logger.error(f"导航进程提前退出，rc={proc.returncode}，输出: {combined[:300]}")
self._result_status = "failed"
result_received = True
break
if self._cancel_event.is_set():
break
# 清理
stop_reading.set() stop_reading.set()
if act_proc.poll() is None: if proc.poll() is None:
act_proc.terminate() proc.terminate()
try: try:
act_proc.wait(timeout=3) proc.wait(timeout=3)
except subprocess.TimeoutExpired: except subprocess.TimeoutExpired:
act_proc.kill() proc.kill()
t_out.join(timeout=2)
out_thread.join(timeout=2) t_err.join(timeout=2)
err_thread.join(timeout=2)
# 处理取消
if self._cancel_event.is_set() and not result_received: if self._cancel_event.is_set() and not result_received:
logger.info("取消 Nav2 导航...") logger.info("取消 Nav2 导航...")
cancel_cmd = f'bash -l -c \'{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true\'' setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
cancel_cmd = f'bash -l -c \'{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || true\''
subprocess.run(cancel_cmd, shell=True, timeout=3) subprocess.run(cancel_cmd, shell=True, timeout=3)
self._result_status = "cancelled" self._result_status = "cancelled"
@@ -429,7 +296,7 @@ class Nav2Navigator:
def navigate_through_poses(self, poses: List[Tuple[float, float, float]], def navigate_through_poses(self, poses: List[Tuple[float, float, float]],
timeout_per_pose: float = 120.0, timeout_per_pose: float = 120.0,
blocking: bool = True) -> bool: blocking: bool = True) -> bool:
"""导航通过多个路径点（Nav2 navigate_through_poses action）""" """导航通过多个路径点"""
if self.status == Nav2Status.NAVIGATING: if self.status == Nav2Status.NAVIGATING:
logger.warning("导航正在进行中，请先停止") logger.warning("导航正在进行中，请先停止")
return False return False
@@ -461,7 +328,6 @@ class Nav2Navigator:
try: try:
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1" setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
# 构建 poses heredoc
poses_lines = [] poses_lines = []
for i, (x, y, yaw) in enumerate(poses): for i, (x, y, yaw) in enumerate(poses):
qz = math.sin(yaw / 2.0) qz = math.sin(yaw / 2.0)
@@ -493,6 +359,7 @@ class Nav2Navigator:
) )
stdout_lines = [] stdout_lines = []
stderr_lines = []
stop_reading = threading.Event() stop_reading = threading.Event()
def reader(pipe, lines): def reader(pipe, lines):
@@ -502,14 +369,16 @@ class Nav2Navigator:
lines.append(line) lines.append(line)
out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines)) out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines))
err_thread = threading.Thread(target=reader, args=(process.stderr, stderr_lines))
out_thread.start() out_thread.start()
err_thread.start()
try: try:
process.stdin.write(poses_yaml + "\n") process.stdin.write(poses_yaml + "\n")
process.stdin.flush() process.stdin.flush()
process.stdin.close() process.stdin.close()
except Exception as e: except Exception as e:
logger.error(f"写入 poses heredoc 失败: {e}") logger.error(f"写入 poses YAML 失败: {e}")
self.status = Nav2Status.FAILED self.status = Nav2Status.FAILED
return return
@@ -520,29 +389,17 @@ class Nav2Navigator:
full_err = "".join(stderr_lines) full_err = "".join(stderr_lines)
combined = full_out + full_err combined = full_out + full_err
result_idx = combined.find("Result:") parsed = self._parse_result_status(combined)
if result_idx >= 0: if parsed:
import re self._result_status = parsed
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:]) result_received = True
if status_m: if parsed == "succeeded":
st = status_m.group(1).lower() logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
if st in ('succeeded', 'SUCCEEDED'): break
logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 路径点导航失败")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
self._result_status = "cancelled"
result_received = True
break
if process.poll() is not None and not result_received: if process.poll() is not None and not result_received:
self._result_status = "failed" self._result_status = "failed"
result_received = True
break break
stop_reading.set() stop_reading.set()
@@ -550,6 +407,7 @@ class Nav2Navigator:
process.terminate() process.terminate()
process.wait(timeout=3) process.wait(timeout=3)
out_thread.join(timeout=2) out_thread.join(timeout=2)
err_thread.join(timeout=2)
if self._result_status == "succeeded": if self._result_status == "succeeded":
self.status = Nav2Status.SUCCEEDED self.status = Nav2Status.SUCCEEDED