Nav2 导航集成：新增 nav2_navigator.py，mission_executor 改用 Nav2 action 导航

- 新增 agv_app/utils/nav2_navigator.py：Nav2Navigator 类，通过 ros2 action
  /navigate_to_pose 和 /navigate_through_poses 与 Nav2 通信，支持路径点导航
- app.py：navigate/to, navigate/path, navigate/status 三个 API 改用 Nav2Navigator
- mission_executor.py：_execute_point 中调用 _nav2_go_to_point() 替代原来的
  time.sleep 模拟移动，Nav2 负责从当前点到目标点的自主导航
- 原有的 map_navigator.py（A* + Pure Pursuit 自实现）保留不动，供无 Nav2 时降级用

agv_app/app.py

@@ -17,7 +17,7 @@ from utils.agv_controller_ros2 import AGVController
 from utils.qr_scanner import QRScanner
 from utils.image_uploader import ImageUploader
 from utils.mission_executor import MissionExecutor, TaskStatus
-from utils.map_navigator import MapNavigator, NavStatus
+from utils.nav2_navigator import Nav2Navigator, Nav2Status
 
 # 配置日志
 logging.basicConfig(
@@ -51,7 +51,7 @@ class GlobalState:
             "positions": []               # 独立点位配置 [{row, col, side, coords, poses}]
         }
         self.machines_config = []        # 机器配置（每台机器的正面/背面点位+姿态）
-        self.navigator = None              # MapNavigator 实例
+        self.navigator = None              # Nav2Navigator 实例
         self.lock = threading.Lock()
 
     def reset(self):
@@ -412,12 +412,13 @@ def api_navigate_to():
 
     try:
         if gs.navigator is None:
-            gs.navigator = MapNavigator(gs.map_config["map_yaml"])
+            gs.navigator = Nav2Navigator()
-        ok = gs.navigator.navigate_to(float(goal_x), float(goal_y))
+        # navigate_to_pose(x, y, yaw=None, timeout_sec=120, blocking=False)
+        ok = gs.navigator.navigate_to_pose(float(goal_x), float(goal_y), blocking=False)
         if ok:
             return jsonify({"ok": True, "message": "导航已启动"})
         else:
-            return jsonify({"ok": False, "error": "路径规划失败或导航正在进行中"}), 400
+            return jsonify({"ok": False, "error": "导航启动失败，可能是Nav2未运行或AGV未连接"}), 400
     except Exception as e:
         logger.error(f"导航失败: {e}")
         return jsonify({"ok": False, "error": str(e)}), 500
@@ -437,12 +438,12 @@ def api_navigate_status():
     """获取导航状态"""
     if gs.navigator:
         return jsonify(gs.navigator.get_status())
-    return jsonify({"status": "idle", "current_position": [0, 0, 0], "path_length": 0, "path": []})
+    return jsonify({"status": "idle", "current_position": [0, 0, 0], "nav2_available": False})
 
 
 @app.route("/api/navigate/path", methods=["POST"])
 def api_navigate_path():
-    """预览路径（仅规划不执行）"""
+    """预览路径（仅规划不执行）- Nav2版本不支持预计算路径，返回当前导航状态"""
     if not gs.map_config or "map_yaml" not in gs.map_config:
         return jsonify({"ok": False, "error": "地图未加载"}), 400
 
@@ -454,12 +455,16 @@ def api_navigate_path():
 
     try:
         if gs.navigator is None:
-            gs.navigator = MapNavigator(gs.map_config["map_yaml"])
+            gs.navigator = Nav2Navigator()
-        path = gs.navigator.get_path_preview(float(goal_x), float(goal_y))
+        # Nav2 不提供路径预览，直接返回可用状态
-        if path is not None:
+        current = gs.navigator.get_current_position()
-            return jsonify({"ok": True, "path": path, "length": len(path)})
+        status = gs.navigator.get_status()
-        else:
+        return jsonify({
-            return jsonify({"ok": False, "error": "路径规划失败，目标点不可达"}), 400
+            "ok": True,
+            "message": "Nav2 路径预览不可用，请在 RViz 中查看规划路径",
+            "current_position": current,
+            "nav2_available": status.get("nav2_available", False)
+        })
     except Exception as e:
         logger.error(f"路径预览失败: {e}")
         return jsonify({"ok": False, "error": str(e)}), 500

agv_app/data/models_config.json

@@ -0,0 +1,71 @@
+[
+  {
+    "id": "m_1778767289",
+    "name": "机型1",
+    "serial_prefix": "",
+    "poses": [
+      {
+        "id": "pose_1778767302",
+        "name": "front_1",
+        "photo_type": "front",
+        "arm_angles": [
+          0,
+          0,
+          0,
+          0,
+          0,
+          0
+        ],
+        "speed": 500,
+        "description": ""
+      },
+      {
+        "id": "pose_1778767312",
+        "name": "front_2",
+        "photo_type": "front",
+        "arm_angles": [
+          0,
+          0,
+          0,
+          0,
+          0,
+          0
+        ],
+        "speed": 500,
+        "description": ""
+      },
+      {
+        "id": "pose_1778767323",
+        "name": "back_1",
+        "photo_type": "back",
+        "arm_angles": [
+          0,
+          0,
+          0,
+          0,
+          0,
+          0
+        ],
+        "speed": 500,
+        "description": ""
+      },
+      {
+        "id": "pose_1778767325",
+        "name": "back_2",
+        "photo_type": "back",
+        "arm_angles": [
+          0,
+          0,
+          0,
+          0,
+          0,
+          0
+        ],
+        "speed": 500,
+        "description": ""
+      }
+    ],
+    "description": "",
+    "notes": ""
+  }
+]

agv_app/timeout

@@ -0,0 +1 @@
+bash: line 1: /dev/null1: Permission denied

agv_app/utils/mission_executor.py

@@ -5,16 +5,21 @@ import os
 import json
 import time
 import logging
+import subprocess
+import math
 from typing import List, Dict, Optional
 from enum import Enum
 
 from .arm_client import ArmClient
-from .agv_controller import AGVController
+from .agv_controller_ros2 import AGVController
 from .qr_scanner import QRScanner
 from .image_uploader import ImageUploader
 
 logger = logging.getLogger(__name__)
 
+# ROS2 环境设置（与 agv_controller_ros2.py 保持一致）
+ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
+
 
 class TaskStatus(Enum):
     PENDING = "pending"
@@ -122,12 +127,16 @@ class MissionExecutor:
             "poses": []
         }
 
-        # 1. AGV 移动到点位
+        # 1. AGV 移动到点位（Nav2 导航）
         coords = point.get("coords", {})
         x, y = coords.get("x", 0), coords.get("y", 0)
-        logger.info(f"AGV 移动到 ({x}, {y})")
+        rz = coords.get("rz", 0.0)  # 目标朝向
-        # TODO: 调用导航移动到目标点
+        logger.info(f"AGV Nav2 导航到 ({x}, {y}), yaw={rz}")
-        time.sleep(1)  # 模拟移动
+
+        nav_result = self._nav2_go_to_point(x, y, rz)
+        if not nav_result:
+            logger.warning(f"AGV 导航到点位 {point_name} 失败，跳过")
+            return {"point_name": point_name, "status": "failed", "error": "导航失败"}
 
         # 2. 执行该点位的所有姿态
         poses = point.get("poses", [])
@@ -206,6 +215,151 @@ class MissionExecutor:
         self.uploader.upload(photo_path, serial, pose_idx, photo_type)
         logger.info(f"上传完成: {serial} {photo_type}")
 
+    # ========== Nav2 导航 ==========
+
+    def _nav2_check_available(self) -> bool:
+        """检查 Nav2 action server 是否可用"""
+        try:
+            rc, out, err = self._run_ros2_cmd("ros2 action list")
+            if rc != 0:
+                return False
+            return "/navigate_to_pose" in out or "navigate_to_pose" in out
+        except:
+            return False
+
+    def _nav2_send_goal(self, x: float, y: float, yaw: float, timeout_sec: float = 120.0) -> bool:
+        """
+        通过 ros2 action 发送 navigate_to_pose 目标并等待结果
+
+        Returns:
+            是否成功到达
+        """
+        # 检查 Nav2 可用性
+        if not self._nav2_check_available():
+            logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
+            return False
+
+        # 构建四元数
+        qz = math.sin(yaw / 2.0)
+        qw = math.cos(yaw / 2.0)
+
+        pose_yaml = (
+            f"pose:\n"
+            f"  header:\n"
+            f"    stamp:\n"
+            f"      sec: 0\n"
+            f"      nanosec: 0\n"
+            f"    frame_id: 'map'\n"
+            f"  position:\n"
+            f"    x: {x}\n"
+            f"    y: {y}\n"
+            f"    z: 0.0\n"
+            f"  orientation:\n"
+            f"    x: 0.0\n"
+            f"    y: 0.0\n"
+            f"    z: {qz}\n"
+            f"    w: {qw}"
+        )
+
+        logger.info(f"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
+
+        # 发送 goal 并监听反馈和结果
+        cmd = (
+            f"ros2 action send_goal /navigate_to_pose "
+            f"navigation_action_msgs/NavigateToPose "
+            f"'{pose_yaml}' "
+            f"--feedback"
+        )
+
+        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+
+        try:
+            process = subprocess.Popen(
+                full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
+            )
+
+            succeeded = False
+            check_interval = 1.0
+            elapsed = 0.0
+
+            while elapsed < timeout_sec:
+                import select
+                reads = [process.stdout]
+                ready, _, _ = select.select(reads, [], [], check_interval)
+                elapsed += check_interval
+
+                if process.stdout in ready:
+                    line = process.stdout.readline()
+                    if not line:
+                        break
+                    line_stripped = line.strip()
+
+                    if "succeeded" in line_stripped.lower():
+                        logger.info("✅ Nav2 导航成功到达目标")
+                        succeeded = True
+                        break
+                    elif "failed" in line_stripped.lower() or "aborted" in line_stripped.lower():
+                        logger.warning(f"⚠️ Nav2 导航失败: {line_stripped}")
+                        break
+                    elif "canceled" in line_stripped.lower() or "cancelled" in line_stripped.lower():
+                        logger.info("Nav2 导航被取消")
+                        break
+
+                if process.poll() is not None:
+                    # 进程结束但没读到 succeeded
+                    break
+
+            # 确保进程结束
+            if process.poll() is None:
+                process.terminate()
+                try:
+                    process.wait(timeout=3)
+                except subprocess.TimeoutExpired:
+                    process.kill()
+
+            return succeeded
+
+        except Exception as e:
+            logger.error(f"Nav2 导航异常: {e}")
+            return False
+
+    def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
+                          timeout_sec: float = 120.0) -> bool:
+        """
+        AGV 通过 Nav2 导航到目标点位
+
+        Args:
+            x, y:    目标世界坐标（米）
+            yaw:     目标朝向（弧度）
+            timeout_sec: 超时时间
+
+        Returns:
+            是否成功到达
+        """
+        return self._nav2_send_goal(x, y, yaw, timeout_sec)
+
+    def _nav2_cancel(self):
+        """取消当前 Nav2 导航"""
+        cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
+        try:
+            subprocess.run(cancel_cmd, shell=True, timeout=3)
+            logger.info("Nav2 导航已取消")
+        except:
+            pass
+
+    def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
+        """执行 ros2 命令"""
+        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+        try:
+            result = subprocess.run(
+                full_cmd, shell=True, capture_output=True, text=True, timeout=timeout
+            )
+            return result.returncode, result.stdout.strip(), result.stderr.strip()
+        except subprocess.TimeoutExpired:
+            return -1, "", "Timeout"
+        except Exception as e:
+            return -1, "", str(e)
+
     # ========== 状态查询 ==========
 
     def get_status(self) -> dict:

agv_app/utils/nav2_navigator.py

@@ -0,0 +1,461 @@
+"""
+Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航
+使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action
+
+依赖：ROS2 humble, nav2_bringup 已启动
+不依赖：自己实现 A*、路径跟踪
+"""
+
+import time
+import math
+import logging
+import threading
+import subprocess
+from typing import List, Tuple, Optional, Dict
+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"
+
+
+class Nav2Status(Enum):
+    IDLE = "idle"
+    NAVIGATING = "navigating"
+    SUCCEEDED = "succeeded"
+    FAILED = "failed"
+    CANCELLED = "cancelled"
+    UNKNOWN = "unknown"
+
+
+class Nav2Navigator:
+    """Nav2 导航器 — 通过 ROS2 Action Server 发送导航目标"""
+
+    def __init__(self):
+        self.status = Nav2Status.IDLE
+        self._nav_thread: Optional[threading.Thread] = None
+        self._cancel_event = threading.Event()
+        self._result_status = None
+        self._current_pose = [0.0, 0.0, 0.0]
+
+    def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> Tuple[int, str, str]:
+        """执行 ros2 命令"""
+        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+        try:
+            result = subprocess.run(
+                full_cmd, shell=True, capture_output=True, text=True, timeout=timeout
+            )
+            return result.returncode, result.stdout.strip(), result.stderr.strip()
+        except subprocess.TimeoutExpired:
+            return -1, "", "Timeout"
+        except Exception as e:
+            return -1, "", str(e)
+
+    def _check_nav2_available(self) -> bool:
+        """检查 Nav2 action server 是否可用"""
+        rc, out, err = self._run_ros2_cmd("ros2 action list")
+        if rc != 0:
+            return False
+        return "/navigate_to_pose" in out or "navigate_to_pose" in out
+
+    def _get_current_pose(self) -> List[float]:
+        """从 /odom 获取当前位置 [x, y, yaw]"""
+        try:
+            rc, out, err = self._run_ros2_cmd(
+                "timeout 5 ros2 topic echo /odom --once 2>/dev/null",
+                timeout=6
+            )
+            if rc == 0 and out:
+                import re, yaml
+                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 = yaml_str.split('---')[0]
+                data = yaml.safe_load(yaml_str)
+                if data:
+                    pos = data.get("pose", {}).get("pose", {}).get("position", {})
+                    orient = data.get("pose", {}).get("pose", {}).get("orientation", {})
+                    x = pos.get("x", 0.0)
+                    y = pos.get("y", 0.0)
+                    qz = orient.get("z", 0.0)
+                    qw = orient.get("w", 1.0)
+                    yaw = math.atan2(2.0 * qw * qz, 1.0 - 2.0 * qz * qz)
+                    self._current_pose = [x, y, yaw]
+                    return self._current_pose
+        except Exception as e:
+            logger.debug(f"获取 odom 失败: {e}")
+        return self._current_pose
+
+    def navigate_to_pose(self, x: float, y: float, yaw: float = None,
+                        timeout_sec: float = 120.0,
+                        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:
+            logger.warning("导航正在进行中，请先停止当前导航")
+            return False
+
+        # 检查 Nav2 是否可用
+        if not self._check_nav2_available():
+            logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
+            return False
+
+        # 构建 pose 消息（geometry_msgs/Pose）
+        if yaw is None:
+            current = self._get_current_pose()
+            yaw = current[2]
+
+        # 计算四元数（只有 yaw）
+        qz = math.sin(yaw / 2.0)
+        qw = math.cos(yaw / 2.0)
+
+        pose_yaml = (
+            f"pose:\\n"
+            f"  header:\\n"
+            f"    stamp:\\n"
+            f"      sec: 0\\n"
+            f"      nanosec: 0\\n"
+            f"    frame_id: 'map'\\n"
+            f"  position:\\n"
+            f"    x: {x}\\n"
+            f"    y: {y}\\n"
+            f"    z: 0.0\\n"
+            f"  orientation:\\n"
+            f"    x: 0.0\\n"
+            f"    y: 0.0\\n"
+            f"    z: {qz}\\n"
+            f"    w: {qw}"
+        )
+
+        logger.info(f"发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
+
+        # 启动导航线程
+        self._cancel_event.clear()
+        self._result_status = None
+        self.status = Nav2Status.NAVIGATING
+
+        self._nav_thread = threading.Thread(
+            target=self._nav_thread_func,
+            args=(x, y, yaw, timeout_sec),
+            daemon=True
+        )
+        self._nav_thread.start()
+
+        if blocking:
+            self._nav_thread.join(timeout=timeout_sec + 10)
+            return self.status == Nav2Status.SUCCEEDED
+
+        return True
+
+    def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float):
+        """导航执行线程"""
+        try:
+            # 计算四元数
+            qz = math.sin(yaw / 2.0)
+            qw = math.cos(yaw / 2.0)
+
+            # 使用 ros2 action send_goal 发送导航目标
+            # Nav2 的 navigate_to_pose action 接受 geometry_msgs/PoseStamped
+            pose_yaml = (
+                f"pose:\\n"
+                f"  header:\\n"
+                f"    stamp:\\n"
+                f"      sec: 0\\n"
+                f"      nanosec: 0\\n"
+                f"    frame_id: 'map'\\n"
+                f"  position:\\n"
+                f"    x: {x}\\n"
+                f"    y: {y}\\n"
+                f"    z: 0.0\\n"
+                f"  orientation:\\n"
+                f"    x: 0.0\\n"
+                f"    y: 0.0\\n"
+                f"    z: {qz}\\n"
+                f"    w: {qw}"
+            )
+
+            # 发送 goal，保持连接获取结果
+            cmd = (
+                f"ros2 action send_goal /navigate_to_pose "
+                f"navigation_action_msgs/NavigateToPose "
+                f"'{pose_yaml}' "
+                f"--feedback"
+            )
+
+            full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+
+            # 用 popen 持续读取反馈，直到完成或取消
+            process = subprocess.Popen(
+                full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
+            )
+
+            feedback_received = False
+            result_received = False
+            cancel_check_interval = 1.0  # 每秒检查一次取消标志
+
+            while not self._cancel_event.is_set():
+                # 非阻塞读取 stdout
+                import select
+                reads = [process.stdout]
+                ready, _, _ = select.select(reads, [], [], cancel_check_interval)
+
+                if process.stdout in ready:
+                    line = process.stdout.readline()
+                    if not line:
+                        break
+                    line = line.strip()
+
+                    # 解析反馈
+                    if "feedback" in line.lower() or "distance_remaining" in line:
+                        feedback_received = True
+                        logger.debug(f"Nav2 feedback: {line[:100]}")
+                    # 解析结果
+                    if "succeeded" in line.lower():
+                        self._result_status = "succeeded"
+                        result_received = True
+                        logger.info("✅ Nav2 导航成功到达目标")
+                        break
+                    elif "failed" in line.lower() or "aborted" in line.lower():
+                        self._result_status = "failed"
+                        result_received = True
+                        logger.warning(f"⚠️ Nav2 导航失败: {line}")
+                        break
+                    elif "canceled" in line.lower() or "cancelled" in line.lower():
+                        self._result_status = "cancelled"
+                        result_received = True
+                        logger.info("Nav2 导航被取消")
+                        break
+
+                # 检查进程是否结束
+                if process.poll() is not None:
+                    # 进程已结束但没读到结果
+                    if not result_received:
+                        self._result_status = "failed"
+                    break
+
+            # 处理取消或超时
+            if self._cancel_event.is_set() and not result_received:
+                logger.info("取消 Nav2 导航...")
+                # 发送取消命令
+                cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose'"
+                subprocess.run(cancel_cmd, shell=True, timeout=3)
+                self._result_status = "cancelled"
+
+            # 确保进程结束
+            if process.poll() is None:
+                process.terminate()
+                try:
+                    process.wait(timeout=3)
+                except subprocess.TimeoutExpired:
+                    process.kill()
+
+            # 设置最终状态
+            if self._result_status == "succeeded":
+                self.status = Nav2Status.SUCCEEDED
+            elif self._result_status == "cancelled":
+                self.status = Nav2Status.CANCELLED
+            else:
+                self.status = Nav2Status.FAILED
+
+        except Exception as e:
+            logger.error(f"Nav2 导航异常: {e}")
+            self.status = Nav2Status.FAILED
+
+    def navigate_through_poses(self, poses: List[Tuple[float, float, float]],
+                               timeout_per_pose: float = 120.0,
+                               blocking: bool = True) -> bool:
+        """
+        导航通过多个路径点（Nav2 navigate_through_poses action）
+
+        Args:
+            poses:        [(x, y, yaw), ...] 路径点列表
+            timeout_per_pose: 每个点位的超时（秒）
+            blocking:     是否阻塞等待完成
+
+        Returns:
+            是否全部成功
+        """
+        if self.status == Nav2Status.NAVIGATING:
+            logger.warning("导航正在进行中，请先停止")
+            return False
+
+        if not self._check_nav2_available():
+            logger.error("Nav2 action server 不可用")
+            return False
+
+        if not poses:
+            return True
+
+        # 构建 poses 列表
+        poses_yaml_parts = []
+        for i, (x, y, yaw) in enumerate(poses):
+            qz = math.sin(yaw / 2.0)
+            qw = math.cos(yaw / 2.0)
+            poses_yaml_parts.append(
+                f"  - pose:\\n"
+                f"      header:\\n"
+                f"        stamp:\\n"
+                f"          sec: 0\\n"
+                f"          nanosec: 0\\n"
+                f"        frame_id: 'map'\\n"
+                f"      position:\\n"
+                f"        x: {x}\\n"
+                f"        y: {y}\\n"
+                f"        z: 0.0\\n"
+                f"      orientation:\\n"
+                f"        x: 0.0\\n"
+                f"        y: 0.0\\n"
+                f"        z: {qz}\\n"
+                f"        w: {qw}"
+            )
+
+        poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
+        logger.info(f"发送 {len(poses)} 个路径点的导航任务")
+
+        self._cancel_event.clear()
+        self.status = Nav2Status.NAVIGATING
+
+        self._nav_thread = threading.Thread(
+            target=self._nav_thread_func_multi,
+            args=(poses, timeout_per_pose),
+            daemon=True
+        )
+        self._nav_thread.start()
+
+        if blocking:
+            total_timeout = len(poses) * timeout_per_pose + 30
+            self._nav_thread.join(timeout=total_timeout)
+            return self.status == Nav2Status.SUCCEEDED
+
+        return True
+
+    def _nav_thread_func_multi(self, poses: List[Tuple[float, float, float]], timeout_per_pose: float):
+        """多路径点导航线程"""
+        try:
+            poses_yaml_parts = []
+            for i, (x, y, yaw) in enumerate(poses):
+                qz = math.sin(yaw / 2.0)
+                qw = math.cos(yaw / 2.0)
+                poses_yaml_parts.append(
+                    f"  - pose:\\n"
+                    f"      header:\\n"
+                    f"        stamp:\\n"
+                    f"          sec: 0\\n"
+                    f"          nanosec: 0\\n"
+                    f"        frame_id: 'map'\\n"
+                    f"      position:\\n"
+                    f"        x: {x}\\n"
+                    f"        y: {y}\\n"
+                    f"        z: 0.0\\n"
+                    f"      orientation:\\n"
+                    f"        x: 0.0\\n"
+                    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"'{poses_yaml}' "
+                f"--feedback"
+            )
+
+            full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+
+            process = subprocess.Popen(
+                full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
+            )
+
+            result_received = False
+
+            while not self._cancel_event.is_set():
+                import select
+                reads = [process.stdout]
+                ready, _, _ = select.select(reads, [], [], 1.0)
+
+                if process.stdout in ready:
+                    line = process.stdout.readline()
+                    if not line:
+                        break
+                    line = line.strip()
+
+                    if "succeeded" in line.lower():
+                        logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
+                        self._result_status = "succeeded"
+                        result_received = True
+                        break
+                    elif "failed" in line.lower() or "aborted" in line.lower():
+                        logger.warning(f"⚠️ Nav2 路径点导航失败: {line}")
+                        self._result_status = "failed"
+                        result_received = True
+                        break
+                    elif "canceled" in line.lower() or "cancelled" in line.lower():
+                        self._result_status = "cancelled"
+                        result_received = True
+                        break
+
+                if process.poll() is not None:
+                    if not result_received:
+                        self._result_status = "failed"
+                    break
+
+            if self._cancel_event.is_set() and not result_received:
+                logger.info("取消路径点导航...")
+                cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_through_poses'"
+                subprocess.run(cancel_cmd, shell=True, timeout=3)
+                self._result_status = "cancelled"
+
+            if process.poll() is None:
+                process.terminate()
+                try:
+                    process.wait(timeout=3)
+                except subprocess.TimeoutExpired:
+                    process.kill()
+
+            if self._result_status == "succeeded":
+                self.status = Nav2Status.SUCCEEDED
+            elif self._result_status == "cancelled":
+                self.status = Nav2Status.CANCELLED
+            else:
+                self.status = Nav2Status.FAILED
+
+        except Exception as e:
+            logger.error(f"Nav2 路径点导航异常: {e}")
+            self.status = Nav2Status.FAILED
+
+    def stop(self):
+        """取消当前导航"""
+        if self.status == Nav2Status.NAVIGATING:
+            self._cancel_event.set()
+            # 发送取消命令
+            cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
+            subprocess.run(cancel_cmd, shell=True, timeout=3)
+            self.status = Nav2Status.CANCELLED
+            logger.info("导航已停止")
+
+    def get_status(self) -> Dict:
+        """获取导航状态"""
+        current = self._get_current_pose()
+        return {
+            "status": self.status.value,
+            "current_position": current,
+            "nav2_available": self._check_nav2_available()
+        }
+
+    def get_current_position(self) -> List[float]:
+        """获取当前位置 [x, y, yaw]"""
+        return self._get_current_pose()