fix: get_position返回缓存位置防止odom无数据时整体失败；map_navigator ROS_DOMAIN_ID=1

2026-05-16 01:26:51 +08:00
parent 368cfd58b0
commit 924f9f3be6
2 changed files with 6 additions and 1 deletions
@@ -185,8 +185,13 @@ class AGVController:
                    yaw = math.atan2(2.0 * qw * qz, 1.0 - 2.0 * qz * qz)
                    self._position = [x, y, yaw]
                    return self._position
+            elif self._connected:
+                # 连接正常但 odom 暂时无数据，返回缓存位置
+                return self._position
        except Exception as e:
            logger.debug(f"获取位置失败: {e}")
+        if self._connected:
+            return self._position
        return None

    def go_to_point(self, x: float, y: float, rz: float = None, speed: float = 0.5) -> bool: