导航修复

agv_app/start_all.sh

@@ -1,71 +1,104 @@
 #!/bin/bash
 # ============================================================
-# Robot AGV 全量启动脚本
+# Robot AGV 全量启动脚本 v2.2
-# 完整流程：清理旧进程 -> 启动 bringup -> 启动 Nav2 -> 激活 Lifecycle -> 启动 Flask
+# 完整流程：
+#   清理旧进程（不杀 daemon） -> 启动 bringup ->
+#   启动激光时间戳修正节点 -> 启动 Nav2 ->
+#   设置导航精度参数 -> 启动 Flask
 # ============================================================
 set -e
 
-LOG_DIR="/home/elephant/work/agv_app"
+AGV_APP_DIR="/home/elephant/work/agv_app"
-cd "$LOG_DIR"
+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. 清理旧进程 ----------
+# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
-echo "[1/6] 清理旧进程..."
+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 ----------
+# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ----------
-echo "[2/6] 重启 ros2 daemon..."
+echo "[2/7] 重启 ros2 daemon..."
-source /opt/ros/humble/setup.bash
-ros2 daemon stop 2>/dev/null || true
-sleep 2
-nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" &>/dev/null &
-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 &
+nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
 sleep 5
-fi
 echo "   ros2 daemon 已就绪"
 
-# ---------- 3. 启动 bringup ----------
+# ---------- 3. 启动 bringup (含激光雷达) ----------
-echo "[3/6] 启动 AGV Bringup (agv_pro + 激光雷达)..."
+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
+for i in $(seq 1 20); do
-    if bash -c "source /opt/ros/humble/setup.bash && source install/setup.bash && 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 已就绪 (/odom 话题已上线)"
+        echo "   ✅ bringup 已就绪"
         break
     fi
     sleep 2
 done
 
-# ---------- 4. 启动 Nav2 ----------
+# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ----------
-echo "[4/6] 启动 Nav2 导航..."
+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 "   等待 Nav2 节点就绪..."
-echo "[5/6] 等待 Nav2 Lifecycle Manager 就绪..."
+for i in $(seq 1 15); do
-source /opt/ros/humble/setup.bash
+    NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
-cd /home/elephant/agv_pro_ros2
+        grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0)
-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 节点已启动"
     if [ "$NODES" -ge 3 ]; then
-        echo "   ✅ Nav2 节点已就绪"
+        echo "   ✅ Nav2 节点已就绪 ($NODES 个)"
         break
     fi
     sleep 3
 done
 
-# 检查是否已 Active（autostart=True 应该自动激活）
+# ---------- 6. 设置精度参数 ----------
-echo "   检查 Lifecycle 状态..."
+echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
-LIFECYCLE_STATE=$(ros2 lifecycle list /lifecycle_manager_navigation 2>/dev/null | grep "Active\|Inactive" | head -1 || echo "")
+source /opt/ros/humble/setup.bash
-if echo "$LIFECYCLE_STATE" | grep -q "Active"; then
+cd "$AGV_ROS2_DIR"
-    echo "   ✅ Nav2 Lifecycle 已激活 (autostart=True 生效)"
+source install/setup.bash
-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. 启动 Flask ----------
+for NODE in /controller_server /bt_navigator /planner_server; do
-echo "[6/6] 启动 Flask API 服务..."
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
-cd /home/elephant/work/agv_app
+    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 '已退出')"
+for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
-echo "    Nav2    : $(ps aux | grep -w "$NAV2_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
+    NAME="${PROC%%:*}"
-echo "    Flask   : $(ps aux | grep -w "$FLASK_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
+    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 "  API 地址: http://localhost:5000"
+echo "  关键验证命令:"
-echo ""
-echo "  常用检查命令:"
-echo "    curl http://localhost:5000/api/status"
 echo "    curl http://localhost:5000/api/navigate/status"
-echo "    curl -X POST http://localhost:5000/api/mission/init_pose"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
-echo "    curl -X POST http://localhost:5000/api/device/connect -H 'Content-Type: application/json' -d '{\"device\":\"agv\"}'"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
-echo "    curl -X POST http://localhost:5000/api/navigate/to -H 'Content-Type: application/json' -d '{\"x\":-0.249,\"y\":-0.957}'"
-echo ""

agv_app/static/js/setting.js

@@ -22,6 +22,8 @@ createApp({
       nav2Available: false,
       // 点位
       points: [],
+      editingPoint: null,   // 当前编辑的点位 {pointRow, col}
+      pointEditor: { x: 0, y: 0, yaw: 0 },
       newPointName: '',
       newPointMode: 'front',
       newPointSequence: ['front', 'back'],
@@ -722,6 +724,217 @@ createApp({
         return machineBelow.front
       }
       return null
+    },
+
+    // 点位归属标签
+    getPointOwnerLabel(pointRow, col) {
+      var rows = this.missionConfig.rows
+      var labels = []
+      if (pointRow === 0) {
+        var m = this.getMachineAt(0, col)
+        if (m) labels.push('机器' + (m.row + 1) + '正面')
+      } else if (pointRow === rows) {
+        var m2 = this.getMachineAt(rows - 1, col)
+        if (m2) labels.push('机器' + (m2.row + 1) + '背面')
+      } else {
+        var mAbove = this.getMachineAt(pointRow - 1, col)
+        var mBelow = this.getMachineAt(pointRow, col)
+        if (mAbove) labels.push('机器' + (mAbove.row + 1) + '背面')
+        if (mBelow) labels.push('机器' + (mBelow.row + 1) + '正面')
       }
+      if (labels.length === 0) return '第' + (col + 1) + '列 · 无归属'
+      return '第' + (col + 1) + '列 · ' + labels.join('/')
+    },
+
+    canClearPoint(pointRow, col) {
+      var rows = this.missionConfig.rows
+      if (pointRow > 0 && pointRow <= rows) {
+        var mAbove = this.getMachineAt(pointRow - 1, col)
+        if (mAbove) return false
+      }
+      if (pointRow >= 0 && pointRow < rows) {
+        var mBelow = this.getMachineAt(pointRow, col)
+        if (mBelow) return false
+      }
+      return true
+    },
+
+    openPointEdit(pointRow, col) {
+      var existing = this.getPointAt(pointRow, col)
+      if (existing && existing.coords) {
+        this.pointEditor.x = existing.coords[0] !== undefined ? existing.coords[0] : 0
+        this.pointEditor.y = existing.coords[1] !== undefined ? existing.coords[1] : 0
+        this.pointEditor.yaw = existing.coords[2] !== undefined ? existing.coords[2] : 0
+      } else {
+        this.pointEditor.x = 0
+        this.pointEditor.y = 0
+        this.pointEditor.yaw = 0
+      }
+      this.editingPoint = { pointRow: pointRow, col: col }
+    },
+
+    closePointEdit() {
+      this.editingPoint = null
+    },
+
+    async loadPointFromAgv() {
+      if (!this.agvConnected) { alert('请先连接AGV'); return }
+      try {
+        const res = await fetch(API + '/api/agv/position')
+        const pos = await res.json()
+        if (pos.ok && pos.position && Array.isArray(pos.position)) {
+          this.pointEditor.x = pos.position[0] ?? 0
+          this.pointEditor.y = pos.position[1] ?? 0
+          this.pointEditor.yaw = pos.position[2] ?? 0
+          alert('✅ 已读取AGV位置: (' + this.pointEditor.x.toFixed(2) + ', ' + this.pointEditor.y.toFixed(2) + ', ' + this.pointEditor.yaw.toFixed(2) + ')')
+        } else if (pos.ok && (!pos.position || !Array.isArray(pos.position))) {
+          alert('⚠️ AGV 未发布位置数据，请检查 AGV 传感器是否正常')
+        } else {
+          alert('读取AGV位置失败: ' + (pos.error || '未知错误'))
+        }
+      } catch (e) { alert('读取AGV位置失败: ' + e.message) }
+    },
+
+    async savePoint() {
+      if (!this.editingPoint) return
+      var pointRow = this.editingPoint.pointRow
+      var col = this.editingPoint.col
+      var coords = [this.pointEditor.x, this.pointEditor.y, this.pointEditor.yaw]
+      try {
+        var saveRes = await fetch(API + '/api/mission/positions', {
+          method: 'POST',
+          headers: { 'Content-Type': 'application/json' },
+          body: JSON.stringify({
+            row: pointRow, col: col, side: 'shoot', coords: coords, poses: []
+          })
+        })
+        var saveData = await saveRes.json()
+        if (!saveData.ok) { alert('保存点位失败: ' + (saveData.error || '未知错误')); return }
+        this.syncPointToMachines(pointRow, col, coords)
+        await this.loadMissionConfig()
+        alert('✅ 点位坐标已保存: (' + coords[0].toFixed(2) + ', ' + coords[1].toFixed(2) + ', ' + coords[2].toFixed(2) + ')')
+        this.closePointEdit()
+      } catch (e) { alert('保存点位失败: ' + e.message) }
+    },
+
+    syncPointToMachines(pointRow, col, coords) {
+      var rows = this.missionConfig.rows
+      if (pointRow === 0) {
+        var m0 = this.getMachineAt(0, col)
+        if (m0) this.updateMachineSide(m0, 'front', coords)
+        return
+      }
+      if (pointRow === rows) {
+        var mLast = this.getMachineAt(rows - 1, col)
+        if (mLast) this.updateMachineSide(mLast, 'back', coords)
+        return
+      }
+      var mAbove = this.getMachineAt(pointRow - 1, col)
+      var mBelow = this.getMachineAt(pointRow, col)
+      if (mAbove) this.updateMachineSide(mAbove, 'back', coords)
+      if (mBelow) this.updateMachineSide(mBelow, 'front', coords)
+    },
+
+    async updateMachineSide(machine, side, coords) {
+      try {
+        var update = {}
+        update[side] = { coords: coords, poses: (machine[side] && machine[side].poses) ? machine[side].poses : [] }
+        await fetch(API + '/api/mission/machines/' + machine.id, {
+          method: 'PUT', headers: { 'Content-Type': 'application/json' },
+          body: JSON.stringify(update)
+        })
+      } catch (e) { console.error('同步机器坐标失败', e) }
+    },
+
+    async clearPoint() {
+      if (!this.editingPoint) return
+      var pointRow = this.editingPoint.pointRow
+      var col = this.editingPoint.col
+      if (!this.canClearPoint(pointRow, col)) {
+        var ownerLabel = this.getPointOwnerLabel(pointRow, col)
+        alert('⚠️ 无法清空！此点位服务于: ' + ownerLabel + '\n必须先移除相关机器才能清空此点位。')
+        return
+      }
+      if (!confirm('确定清空此点位坐标？')) return
+      try {
+        await fetch(API + '/api/mission/positions/' + pointRow + '/' + col, { method: 'DELETE' })
+        await this.loadMissionConfig()
+        alert('✅ 点位已清空')
+        this.closePointEdit()
+      } catch (e) { alert('清空点位失败: ' + e.message) }
+    },
+
+    async navigateToPoint() {
+      if (!this.editingPoint || !this.pointEditor) return
+      var coords = [this.pointEditor.x, this.pointEditor.y, this.pointEditor.yaw]
+      if (!confirm('确定导航到点位 (' + coords[0].toFixed(2) + ', ' + coords[1].toFixed(2) + ') 吗？')) return
+      try {
+        var res = await fetch(API + '/api/navigate/to', {
+          method: 'POST', headers: { 'Content-Type': 'application/json' },
+          body: JSON.stringify({ x: coords[0], y: coords[1] })
+        })
+        var data = await res.json()
+        if (data.ok) {
+          alert('✅ 导航已启动')
+        } else {
+          alert('❌ ' + (data.error || '导航启动失败'))
+        }
+      } catch (e) { alert('导航请求失败: ' + e.message) }
+    },
+
+    async refreshPoseAngles(modelId, poseId) {
+      if (!this.armConnected) { alert('机械臂未连接'); return }
+      try {
+        var res = await fetch(API + '/api/arm/get_angles')
+        var data = await res.json()
+        if (data.ok && data.angles) {
+          var model = this.models.find(m => m.id === modelId)
+          if (model) {
+            var pose = model.poses.find(p => p.id === poseId)
+            if (pose) {
+              pose.arm_angles = [...data.angles]
+              await fetch(API + '/api/models/' + modelId + '/poses/' + poseId, {
+                method: 'PUT', headers: { 'Content-Type': 'application/json' },
+                body: JSON.stringify({ arm_angles: pose.arm_angles })
+              })
+            }
+          }
+        }
+      } catch (e) { console.error('refreshPoseAngles error:', e) }
+    },
+
+    async applyPoseAngles(modelId, poseId) {
+      var model = this.models.find(m => m.id === modelId)
+      if (model) {
+        var pose = model.poses.find(p => p.id === poseId)
+        if (pose && pose.arm_angles) {
+          try {
+            await fetch(API + '/api/arm/set_angles', {
+              method: 'POST', headers: { 'Content-Type': 'application/json' },
+              body: JSON.stringify({ angles: pose.arm_angles, speed: 500 })
+            })
+          } catch (e) { console.error('applyPoseAngles error:', e) }
+        }
+      }
+    },
+
+    async adjustPoseAngle(modelId, poseId, jointIndex, delta) {
+      var model = this.models.find(m => m.id === modelId)
+      if (model) {
+        var pose = model.poses.find(p => p.id === poseId)
+        if (pose) {
+          if (!pose.arm_angles) pose.arm_angles = [0, 0, 0, 0, 0, 0]
+          pose.arm_angles[jointIndex] = (pose.arm_angles[jointIndex] || 0) + delta
+          await fetch(API + '/api/models/' + modelId + '/poses/' + poseId, {
+            method: 'PUT', headers: { 'Content-Type': 'application/json' },
+            body: JSON.stringify({ arm_angles: pose.arm_angles })
+          })
+          await fetch(API + '/api/arm/set_angle', {
+            method: 'POST', headers: { 'Content-Type': 'application/json' },
+            body: JSON.stringify({ joint: 'J' + (jointIndex + 1), angle: pose.arm_angles[jointIndex] })
+          })
+        }
+      }
+    },
   }
 }).mount('#app')

agv_app/utils/nav2_navigator.py

@@ -162,6 +162,10 @@ class Nav2Navigator:
             logger.warning("导航正在进行中，请先停止当前导航")
             return False
 
+        # 重置状态，允许发起新导航
+        self.status = Nav2Status.IDLE
+        self._result_status = None
+
         if not self._check_nav2_available():
             logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
             return False
@@ -177,6 +181,11 @@ class Nav2Navigator:
         self._result_status = None
         self.status = Nav2Status.NAVIGATING
 
+        # 停掉旧线程（防止重复调用导致多线程冲突）
+        if self._nav_thread and self._nav_thread.is_alive():
+            self.stop()
+            self._nav_thread.join(timeout=3)
+
         self._nav_thread = threading.Thread(
             target=self._nav_thread_func,
             args=(x, y, yaw, timeout_sec),