导航修复

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),