导航修复

agv_app/utils/agv_controller_ros2.py

@@ -44,41 +44,23 @@ class AGVController:
             return -1, "", str(e)
 
     def connect(self) -> bool:
-        """连接 AGV - 检查 ROS2 节点和 topic"""
+        """连接 AGV - 检查 ROS2 topic 是否可用"""
         try:
-            # 检查 agv_pro_node 是否运行
-            rc, out, err = self._run_ros2_cmd("ros2 node list")
-            if rc != 0:
-                logger.error(f"ROS2 节点列表获取失败: {err}")
-                return False
-
-            if "/agv_pro_node" not in out:
-                logger.error("agv_pro_node 未运行")
-                return False
-
-            # 检查 /odom topic
+            # 检查 /odom topic 是否存在（比 ros2 node list 更可靠）
             rc, out, err = self._run_ros2_cmd("ros2 topic list")
+            if rc != 0:
+                logger.error(f"ROS2 topic 列表获取失败: {err}")
+                return False
+
             if "/odom" not in out:
                 logger.error("/odom topic 不存在")
                 return False
 
-            # 尝试获取一次位置数据
-            rc, out, err = self._run_ros2_cmd(
-                "timeout 5 ros2 topic echo /odom --once 2>/dev/null",
-                timeout=6
-            )
-
-            if rc == 0 and out:
-                self._connected = True
-                self._ros2_available = True
-                logger.info("AGV ROS2 连接成功")
-                return True
-            else:
-                # /odom 可能暂时没数据，但节点存在也算连接成功
-                self._connected = True
-                self._ros2_available = True
-                logger.info("AGV ROS2 连接成功 (节点存在，等待 odom 数据)")
-                return True
+            # /odom 存在即表示 AGV 节点在运行
+            self._connected = True
+            self._ros2_available = True
+            logger.info("AGV ROS2 连接成功 (/odom topic 存在)")
+            return True
 
         except Exception as e:
             logger.error(f"AGV 连接失败: {e}")
@@ -154,13 +136,13 @@ class AGVController:
             self._publish_cmd_vel(0, 0, 0)
 
     def get_position(self) -> Optional[List[float]]:
-        """获取 AGV 当前位置 [x, y, yaw]"""
+        """获取 AGV 当前位置 [x, y, yaw]（使用 AMCL 全局定位，比 /odom 更准确）"""
         if not self.is_connected():
             return None
         try:
-            # 从 /odom topic 获取位置
+            # 优先从 /amcl_pose 获取全局定位位置（更准确）
             rc, out, err = self._run_ros2_cmd(
-                "timeout 5 ros2 topic echo /odom --once 2>/dev/null",
+                "timeout 5 ros2 topic echo /amcl_pose --once 2>/dev/null",
                 timeout=6
             )
             if rc == 0 and out: