对齐场地几何与运行模式基础设施

world/commons/field_landmarks.py

@@ -1,14 +1,16 @@
+from __future__ import annotations
+
 import numpy as np
+
 from utils.math_ops import MathOps
 
 
 class FieldLandmarks:
-    def __init__(self, world):
-        from world.world import World  # type hinting
-
-        self.world: World = world
-
-        self.landmarks: dict = {}
+    def __init__(self, field):
+        self.field = field
+        self.world = field.world
+        self.landmarks: dict[str, np.ndarray] = {}
+        self.canonical_landmarks: dict[str, np.ndarray] = field.get_canonical_landmarks()
 
     def update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None:
         """
@@ -21,14 +23,19 @@ class FieldLandmarks:
         global_pos_3d = MathOps.rel_to_global_3d(
             local_pos_3d=local_cart_3d,
             global_pos_3d=world.global_position,
-            global_orientation_quat=world.agent.robot.global_orientation_quat
+            global_orientation_quat=world.agent.robot.global_orientation_quat,
         )
 
         self.landmarks[landmark_id] = global_pos_3d
 
-    def get_landmark_position(self, landmark_id: str) -> np.ndarray | None:
+    def get_landmark_position(
+        self, landmark_id: str, use_canonical: bool = False
+    ) -> np.ndarray | None:
         """
-        Returns the calculated 2d global position for a given landmark ID.
-        Returns None if the landmark is not currently visible or processed.
+        Returns the current perceived or canonical global position for a landmark.
         """
-        return self.global_positions.get(landmark_id)
+        source = self.canonical_landmarks if use_canonical else self.landmarks
+        return source.get(landmark_id)
+
+    def get_canonical_landmark_position(self, landmark_id: str) -> np.ndarray | None:
+        return self.canonical_landmarks.get(landmark_id)