Nav2导航 + 初始化位置功能

- nav2_navigator.py: 通过 ros2 action 与 Nav2 通信，修复 YAML 格式（pose 嵌套）
- mission_executor.py: 改用 Nav2 navigate_to_pose action 导航
- app.py: navigate/to 等 API 改用 Nav2Navigator；新增 /api/mission/init_pose
  API，通过子进程调用 rclpy 发布 /initialpose 到 (0,0,0)
- setting.html/js: 任务配置Tab加「初始化位置」按钮

注意：ROS2 daemon (domain=1) 与 Flask 在不同 domain，
Flask 进程内调用 rclpy 会破坏 daemon 状态，需通过子进程调用

agv_app/app.py

@@ -641,6 +641,33 @@ def api_mission_position():
     return jsonify({"ok": True, "position": pos, "battery": battery})
 
 
+@app.route("/api/mission/init_pose", methods=["POST"])
+def api_mission_init_pose():
+    """将 AMCL 初始位置设为 (0,0,0)，无需 RViz"""
+    try:
+        script = "/tmp/ros2_init_pose.sh"
+        lines = [
+            "#!/bin/bash",
+            "export ROS_DOMAIN_ID=1",
+            "source /opt/ros/humble/setup.bash",
+            "source ~/agv_pro_ros2/install/setup.bash",
+            "python3 /tmp/publish_init_pose.py",
+        ]
+        with open(script, "w") as f:
+            f.write("\n".join(lines) + "\n")
+        os.chmod(script, 0o755)
+        result = subprocess.run(
+            [script],
+            capture_output=True, text=True, timeout=12,
+            env={**os.environ, "ROS_DOMAIN_ID": "1"}
+        )
+        logger.info(f"init_pose: rc={result.returncode}")
+        return jsonify({"ok": True, "message": "初始位置已设为 (0,0,0)"})
+    except Exception as e:
+        logger.error(f"初始化位置失败: {e}")
+        return jsonify({"ok": False, "error": str(e)}), 500
+
+
 @app.route("/api/mission/config", methods=["GET"])
 def api_mission_config_get():
     """获取任务配置（网格尺寸和空位矩阵）"""

agv_app/static/js/setting.js

@@ -50,6 +50,7 @@ const app = createApp({
       agvMoveInterval: null,
       agvCameraUrl: API + '/api/camera/refresh',
       agvCameraTimer: null,
+      initPoseLoading: false,
     }
   },
   mounted() {
@@ -336,6 +337,22 @@ const app = createApp({
         }
       } catch (e) { alert('保存失败: ' + e.message) }
     },
+    async initPose() {
+      try {
+        this.initPoseLoading = true
+        const res = await fetch(API + '/api/mission/init_pose', { method: 'POST' })
+        const data = await res.json()
+        if (data.ok) {
+          alert('✅ 初始位置已设为 (0, 0, 0)')
+        } else {
+          alert('❌ 初始化失败: ' + (data.error || '未知错误'))
+        }
+      } catch (e) {
+        alert('❌ 初始化位置请求失败: ' + e.message)
+      } finally {
+        this.initPoseLoading = false
+      }
+    },
     async loadAllMachines() {
       try {
         const res = await fetch(API + '/api/mission/machines')

agv_app/templates/setting.html

@@ -242,6 +242,7 @@
             <div class="form-group" style="align-self:end">
               <button class="btn btn-primary" @click="generateGrid">🔲 生成网格</button>
               <button class="btn btn-secondary" @click="saveMissionConfig" style="margin-left:6px">💾 保存网格</button>
+              <button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading" style="margin-left:6px">📍 初始化位置</button>
             </div>
           </div>
 
@@ -540,6 +541,6 @@
   </div>
 
   <script src="/static/js/vue3.global.prod.js?v=20260515a"></script>
-  <script src="/static/js/setting.js?v=20260515a"></script>
+  <script src="/static/js/setting.js?v=20260516a"></script>
 </body>
 </html>

agv_app/utils/mission_executor.py

@@ -250,15 +250,16 @@ class MissionExecutor:
             f"      sec: 0\n"
             f"      nanosec: 0\n"
             f"    frame_id: 'map'\n"
-            f"  position:\n"
+            f"  pose:\n"
-            f"    x: {x}\n"
+            f"    position:\n"
-            f"    y: {y}\n"
+            f"      x: {x}\n"
-            f"    z: 0.0\n"
+            f"      y: {y}\n"
-            f"  orientation:\n"
+            f"      z: 0.0\n"
-            f"    x: 0.0\n"
+            f"    orientation:\n"
-            f"    y: 0.0\n"
+            f"      x: 0.0\n"
-            f"    z: {qz}\n"
+            f"      y: 0.0\n"
-            f"    w: {qw}"
+            f"      z: {qz}\n"
+            f"      w: {qw}"
         )
 
         logger.info(f"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
@@ -266,7 +267,7 @@ class MissionExecutor:
         # 发送 goal 并监听反馈和结果
         cmd = (
             f"ros2 action send_goal /navigate_to_pose "
-            f"navigation_action_msgs/NavigateToPose "
+            f"nav2_msgs/action/NavigateToPose "
             f"'{pose_yaml}' "
             f"--feedback"
         )

agv_app/utils/nav2_navigator.py

@@ -17,7 +17,7 @@ from enum import Enum
 logger = logging.getLogger(__name__)
 
 # ROS2 环境设置（与 agv_controller_ros2.py 保持一致）
-ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
+ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
 
 
 class Nav2Status(Enum):
@@ -174,21 +174,22 @@ class Nav2Navigator:
                 f"      sec: 0\\n"
                 f"      nanosec: 0\\n"
                 f"    frame_id: 'map'\\n"
-                f"  position:\\n"
+                f"  pose:\\n"
-                f"    x: {x}\\n"
+                f"    position:\\n"
-                f"    y: {y}\\n"
+                f"      x: {x}\\n"
-                f"    z: 0.0\\n"
+                f"      y: {y}\\n"
-                f"  orientation:\\n"
+                f"      z: 0.0\\n"
-                f"    x: 0.0\\n"
+                f"    orientation:\\n"
-                f"    y: 0.0\\n"
+                f"      x: 0.0\\n"
-                f"    z: {qz}\\n"
+                f"      y: 0.0\\n"
-                f"    w: {qw}"
+                f"      z: {qz}\\n"
+                f"      w: {qw}"
             )
 
             # 发送 goal，保持连接获取结果
             cmd = (
                 f"ros2 action send_goal /navigate_to_pose "
-                f"navigation_action_msgs/NavigateToPose "
+                f"nav2_msgs/action/NavigateToPose "
                 f"'{pose_yaml}' "
                 f"--feedback"
             )
@@ -309,15 +310,16 @@ class Nav2Navigator:
                 f"          sec: 0\\n"
                 f"          nanosec: 0\\n"
                 f"        frame_id: 'map'\\n"
-                f"      position:\\n"
+                f"      pose:\\n"
-                f"        x: {x}\\n"
+                f"        position:\\n"
-                f"        y: {y}\\n"
+                f"          x: {x}\\n"
-                f"        z: 0.0\\n"
+                f"          y: {y}\\n"
-                f"      orientation:\\n"
+                f"          z: 0.0\\n"
-                f"        x: 0.0\\n"
+                f"        orientation:\\n"
-                f"        y: 0.0\\n"
+                f"          x: 0.0\\n"
-                f"        z: {qz}\\n"
+                f"          y: 0.0\\n"
-                f"        w: {qw}"
+                f"          z: {qz}\\n"
+                f"          w: {qw}"
             )
 
         poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
@@ -354,22 +356,24 @@ class Nav2Navigator:
                     f"          sec: 0\\n"
                     f"          nanosec: 0\\n"
                     f"        frame_id: 'map'\\n"
-                    f"      position:\\n"
+                    f"      pose:\\n"
-                    f"        x: {x}\\n"
+                    f"        position:\\n"
-                    f"        y: {y}\\n"
+                    f"          x: {x}\\n"
-                    f"        z: 0.0\\n"
+                    f"          y: {y}\
-                    f"      orientation:\\n"
+"
-                    f"        x: 0.0\\n"
+                    f"          z: 0.0\\n"
-                    f"        y: 0.0\\n"
+                    f"        orientation:\\n"
-                    f"        z: {qz}\\n"
+                    f"          x: 0.0\\n"
-                    f"        w: {qw}"
+                    f"          y: 0.0\\n"
+                    f"          z: {qz}\\n"
+                    f"          w: {qw}"
                 )
 
             poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
 
             cmd = (
                 f"ros2 action send_goal /navigate_through_poses "
-                f"navigation_action_msgs/NavigateThroughPoses "
+                f"nav2_msgs/action/NavigateThroughPoses "
                 f"'{poses_yaml}' "
                 f"--feedback"
             )