Nav2导航 heredoc pipe方式修复 + 重启脚本更新

nav2_navigator.py: 改用 subprocess.Popen + stdin heredoc 避免bash单引号转义
restart_agv.sh: 清理所有旧进程再重启（bringup → Nav2 → Flask）
app.py: init_pose改用 heredoc 子进程脚本方式

agv_app/app.py

@@ -648,7 +648,7 @@ def api_mission_init_pose():
         script = "/tmp/ros2_init_pose.sh"
         lines = [
             "#!/bin/bash",
-            "export ROS_DOMAIN_ID=1",
+            "export ROS_DOMAIN_ID=0",
             "source /opt/ros/humble/setup.bash",
             "source ~/agv_pro_ros2/install/setup.bash",
             "python3 /tmp/publish_init_pose.py",
@@ -659,7 +659,7 @@ def api_mission_init_pose():
         result = subprocess.run(
             [script],
             capture_output=True, text=True, timeout=12,
-            env={**os.environ, "ROS_DOMAIN_ID": "1"}
+            env={**os.environ, "ROS_DOMAIN_ID": "0"}
         )
         logger.info(f"init_pose: rc={result.returncode}")
         return jsonify({"ok": True, "message": "初始位置已设为 (0,0,0)"})

agv_app/utils/agv_controller_ros2.py

@@ -12,7 +12,7 @@ from typing import Tuple, Optional, List
 logger = logging.getLogger(__name__)
 
 # ROS2 环境设置
-ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
+ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash"
 
 
 class AGVController:
@@ -28,7 +28,7 @@ class AGVController:
 
     def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
         """执行 ros2 命令"""
-        full_cmd = f"bash -c 'export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash && {cmd}'"
+        full_cmd = f"bash -l -c 'source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && {cmd}'"
         try:
             result = subprocess.run(
                 full_cmd,

agv_app/utils/mission_executor.py

@@ -18,7 +18,7 @@ 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"
+ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
 
 
 class TaskStatus(Enum):

agv_app/utils/nav2_navigator.py

@@ -2,22 +2,29 @@
 Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航
 使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action
 
-依赖：ROS2 humble, nav2_bringup 已启动
+通过写入临时 shell 脚本再执行，避免 bash -c 单引号转义问题
-不依赖：自己实现 A*、路径跟踪
 """
-
 import time
 import math
 import logging
 import threading
 import subprocess
+import os
 from typing import List, Tuple, Optional, Dict
 from enum import Enum
 
 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"
+def _run_ros2_bash(cmd: str, timeout: float = 5.0) -> Tuple[int, str, str]:
+    """执行 ros2 命令（通过 bash -l 避免环境问题）"""
+    setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash"
+    full_cmd = f"bash -l -c '{setup} && {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"
 
 
 class Nav2Status(Enum):
@@ -30,7 +37,9 @@ class Nav2Status(Enum):
 
 
 class Nav2Navigator:
-    """Nav2 导航器 — 通过 ROS2 Action Server 发送导航目标"""
+    """Nav2 导航器 — 通过 ros2 action /navigate_to_pose 与 Nav2 通信"""
+
+    NAVIGATE_SCRIPT = "/tmp/nav2_send_goal.sh"
 
     def __init__(self):
         self.status = Nav2Status.IDLE
@@ -38,23 +47,51 @@ class Nav2Navigator:
         self._cancel_event = threading.Event()
         self._result_status = None
         self._current_pose = [0.0, 0.0, 0.0]
+        self._build_script()
 
-    def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> Tuple[int, str, str]:
+    def _build_script(self):
-        """执行 ros2 命令"""
+        """生成 Nav2 action 发送脚本（通过 heredoc 避免所有 bash 转义问题）"""
-        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+        script = self.NAVIGATE_SCRIPT
-        try:
+        content = [
-            result = subprocess.run(
+            "#!/bin/bash",
-                full_cmd, shell=True, capture_output=True, text=True, timeout=timeout
+            "source /opt/ros/humble/setup.bash",
-            )
+            "source /home/elephant/agv_pro_ros2/install/setup.bash",
-            return result.returncode, result.stdout.strip(), result.stderr.strip()
+            "",
-        except subprocess.TimeoutExpired:
+            "TARGET_X=${1:-0}",
-            return -1, "", "Timeout"
+            "TARGET_Y=${2:-0}",
-        except Exception as e:
+            "TARGET_YAW=${3:-0}",
-            return -1, "", str(e)
+            "",
+            "# 计算四元数",
+            "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:
         """检查 Nav2 action server 是否可用"""
-        rc, out, err = self._run_ros2_cmd("ros2 action list")
+        rc, out, err = _run_ros2_bash("ros2 action list")
         if rc != 0:
             return False
         return "/navigate_to_pose" in out or "navigate_to_pose" in out
@@ -62,10 +99,7 @@ class Nav2Navigator:
     def _get_current_pose(self) -> List[float]:
         """从 /odom 获取当前位置 [x, y, yaw]"""
         try:
-            rc, out, err = self._run_ros2_cmd(
+            rc, out, err = _run_ros2_bash("timeout 3 ros2 topic echo /odom --once 2>/dev/null", timeout=4)
-                "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)
@@ -106,38 +140,14 @@ class Nav2Navigator:
             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}°")
 
         # 启动导航线程
@@ -159,101 +169,100 @@ class Nav2Navigator:
         return True
 
     def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float):
-        """导航执行线程"""
+        """导航执行线程 — 使用 heredoc 脚本避免 bash 转义问题"""
         try:
-            # 计算四元数
+            logger.info(f"Nav2 线程启动: x={x}, y={y}, yaw={yaw}, timeout={timeout_sec}s")
+            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"
             qz = math.sin(yaw / 2.0)
             qw = math.cos(yaw / 2.0)
 
-            # 使用 ros2 action send_goal 发送导航目标
+            # 构建 heredoc 内容
-            # Nav2 的 navigate_to_pose action 接受 geometry_msgs/PoseStamped
+            heredoc = (
-            pose_yaml = (
+                f"pose:\n"
-                f"pose:\\n"
+                f"  header:\n"
-                f"  header:\\n"
+                f"    stamp:\n"
-                f"    stamp:\\n"
+                f"      sec: 0\n"
-                f"      sec: 0\\n"
+                f"      nanosec: 0\n"
-                f"      nanosec: 0\\n"
+                f"    frame_id: map\n"
-                f"    frame_id: 'map'\\n"
+                f"  pose:\n"
-                f"  pose:\\n"
+                f"    position:\n"
-                f"    position:\\n"
+                f"      x: {x}\n"
-                f"      x: {x}\\n"
+                f"      y: {y}\n"
-                f"      y: {y}\\n"
+                f"      z: 0.0\n"
-                f"      z: 0.0\\n"
+                f"    orientation:\n"
-                f"    orientation:\\n"
+                f"      x: 0.0\n"
-                f"      x: 0.0\\n"
+                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}"
             )
 
-            # 发送 goal，保持连接获取结果
+            cmd = f'{setup} && ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose - --feedback'
-            cmd = (
-                f"ros2 action send_goal /navigate_to_pose "
-                f"nav2_msgs/action/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
+                cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
             )
 
-            feedback_received = False
+            stdout_lines = []
-            result_received = False
+            stderr_lines = []
-            cancel_check_interval = 1.0  # 每秒检查一次取消标志
+            stop_reading = threading.Event()
 
-            while not self._cancel_event.is_set():
+            def reader(pipe, lines):
-                # 非阻塞读取 stdout
+                for line in iter(pipe.readline, ''):
-                import select
+                    if stop_reading.is_set():
-                reads = [process.stdout]
+                        break
-                ready, _, _ = select.select(reads, [], [], cancel_check_interval)
+                    lines.append(line)
 
-                if process.stdout in ready:
+            out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines))
-                    line = process.stdout.readline()
+            err_thread = threading.Thread(target=reader, args=(process.stderr, stderr_lines))
-                    if not line:
+            out_thread.start()
-                        break
+            err_thread.start()
-                    line = line.strip()
 
-                    # 解析反馈
+            # 写入 heredoc 数据
-                    if "feedback" in line.lower() or "distance_remaining" in line:
+            try:
-                        feedback_received = True
+                process.stdin.write(heredoc + "\n")
-                        logger.debug(f"Nav2 feedback: {line[:100]}")
+                process.stdin.flush()
-                    # 解析结果
+                process.stdin.close()
-                    if "succeeded" in line.lower():
+            except Exception as e:
-                        self._result_status = "succeeded"
+                logger.error(f"写入 heredoc 失败: {e}")
-                        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:
+            while not self._cancel_event.is_set() and elapsed < timeout_sec:
-                    # 进程已结束但没读到结果
+                time.sleep(check_interval)
-                    if not result_received:
+                elapsed = time.time() - start_time
-                        self._result_status = "failed"
+
+                full_out = "".join(stdout_lines)
+                if "succeeded" in full_out.lower():
+                    logger.info("✅ Nav2 导航成功到达目标")
+                    self._result_status = "succeeded"
+                    result_received = True
+                    break
+                if any(k in full_out.lower() for k in ["failed", "aborted"]):
+                    logger.warning(f"⚠️ Nav2 导航失败: {full_out[-200:]}")
+                    self._result_status = "failed"
+                    result_received = True
+                    break
+                if any(k in full_out.lower() for k in ["canceled", "cancelled"]):
+                    logger.info("Nav2 导航被取消")
+                    self._result_status = "cancelled"
+                    result_received = True
                     break
 
-            # 处理取消或超时
+                # 检查进程是否结束但没读到结果
-            if self._cancel_event.is_set() and not result_received:
+                if process.poll() is not None and not result_received:
-                logger.info("取消 Nav2 导航...")
+                    if full_out:
-                # 发送取消命令
+                        logger.warning(f"Nav2 进程意外结束，输出: {full_out[:200]}")
-                cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose'"
+                    self._result_status = "failed"
-                subprocess.run(cancel_cmd, shell=True, timeout=3)
+                    break
-                self._result_status = "cancelled"
 
-            # 确保进程结束
+            # 善后
+            stop_reading.set()
             if process.poll() is None:
                 process.terminate()
                 try:
@@ -261,7 +270,15 @@ class Nav2Navigator:
                 except subprocess.TimeoutExpired:
                     process.kill()
 
-            # 设置最终状态
+            out_thread.join(timeout=2)
+            err_thread.join(timeout=2)
+
+            if self._cancel_event.is_set() and not result_received:
+                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'"
+                subprocess.run(cancel_cmd, shell=True, timeout=3)
+                self._result_status = "cancelled"
+
             if self._result_status == "succeeded":
                 self.status = Nav2Status.SUCCEEDED
             elif self._result_status == "cancelled":
@@ -276,55 +293,17 @@ class Nav2Navigator:
     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）"""
-        导航通过多个路径点（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"      pose:\\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
 
@@ -339,96 +318,94 @@ class Nav2Navigator:
             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 = []
+            setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash"
+
+            # 构建 poses heredoc
+            poses_lines = []
             for i, (x, y, yaw) in enumerate(poses):
                 qz = math.sin(yaw / 2.0)
                 qw = math.cos(yaw / 2.0)
-                poses_yaml_parts.append(
+                poses_lines.extend([
-                    f"  - pose:\\n"
+                    f"  - pose:",
-                    f"      header:\\n"
+                    f"      header:",
-                    f"        stamp:\\n"
+                    f"        stamp:",
-                    f"          sec: 0\\n"
+                    f"          sec: 0",
-                    f"          nanosec: 0\\n"
+                    f"          nanosec: 0",
-                    f"        frame_id: 'map'\\n"
+                    f"        frame_id: map",
-                    f"      pose:\\n"
+                    f"      pose:",
-                    f"        position:\\n"
+                    f"        position:",
-                    f"          x: {x}\\n"
+                    f"          x: {x}",
-                    f"          y: {y}\
+                    f"          y: {y}",
-"
+                    f"          z: 0.0",
-                    f"          z: 0.0\\n"
+                    f"        orientation:",
-                    f"        orientation:\\n"
+                    f"          x: 0.0",
-                    f"          x: 0.0\\n"
+                    f"          y: 0.0",
-                    f"          y: 0.0\\n"
+                    f"          z: {qz}",
-                    f"          z: {qz}\\n"
+                    f"          w: {qw}",
-                    f"          w: {qw}"
+                ])
-                )
+            poses_yaml = "poses:\n" + "\n".join(poses_lines)
 
-            poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
+            cmd = f'{setup} && ros2 action send_goal /navigate_through_poses nav2_msgs/action/NavigateThroughPoses - --feedback'
-
-            cmd = (
-                f"ros2 action send_goal /navigate_through_poses "
-                f"nav2_msgs/action/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
+                cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
             )
 
+            stdout_lines = []
+            stop_reading = threading.Event()
+
+            def reader(pipe, lines):
+                for line in iter(pipe.readline, ''):
+                    if stop_reading.is_set():
+                        break
+                    lines.append(line)
+
+            out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines))
+            out_thread.start()
+
+            try:
+                process.stdin.write(poses_yaml + "\n")
+                process.stdin.flush()
+                process.stdin.close()
+            except Exception as e:
+                logger.error(f"写入 poses heredoc 失败: {e}")
+                self.status = Nav2Status.FAILED
+                return
+
             result_received = False
-
             while not self._cancel_event.is_set():
-                import select
+                time.sleep(1.0)
-                reads = [process.stdout]
+                full_out = "".join(stdout_lines)
-                ready, _, _ = select.select(reads, [], [], 1.0)
 
-                if process.stdout in ready:
+                if "succeeded" in full_out.lower():
-                    line = process.stdout.readline()
+                    logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
-                    if not line:
+                    self._result_status = "succeeded"
-                        break
+                    result_received = True
-                    line = line.strip()
+                    break
-
+                if any(k in full_out.lower() for k in ["failed", "aborted"]):
-                    if "succeeded" in line.lower():
+                    logger.warning(f"⚠️ Nav2 路径点导航失败")
-                        logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
+                    self._result_status = "failed"
-                        self._result_status = "succeeded"
+                    result_received = True
-                        result_received = True
+                    break
-                        break
+                if any(k in full_out.lower() for k in ["canceled", "cancelled"]):
-                    elif "failed" in line.lower() or "aborted" in line.lower():
+                    self._result_status = "cancelled"
-                        logger.warning(f"⚠️ Nav2 路径点导航失败: {line}")
+                    result_received = True
-                        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:
+                if process.poll() is not None and not result_received:
-                logger.info("取消路径点导航...")
+                    self._result_status = "failed"
-                cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_through_poses'"
+                    break
-                subprocess.run(cancel_cmd, shell=True, timeout=3)
-                self._result_status = "cancelled"
 
+            stop_reading.set()
             if process.poll() is None:
                 process.terminate()
-                try:
+                process.wait(timeout=3)
-                    process.wait(timeout=3)
+            out_thread.join(timeout=2)
-                except subprocess.TimeoutExpired:
-                    process.kill()
 
             if self._result_status == "succeeded":
                 self.status = Nav2Status.SUCCEEDED
@@ -445,8 +422,8 @@ class Nav2Navigator:
         """取消当前导航"""
         if self.status == Nav2Status.NAVIGATING:
             self._cancel_event.set()
-            # 发送取消命令
+            setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash"
-            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'"
+            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'"
             subprocess.run(cancel_cmd, shell=True, timeout=3)
             self.status = Nav2Status.CANCELLED
             logger.info("导航已停止")