Init

world/commons/field_landmarks.py

@@ -0,0 +1,34 @@
+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 update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None:
+        """
+        Calculates the global position of all currently visible landmarks.
+        """
+
+        world = self.world
+        local_cart_3d = MathOps.deg_sph2cart(landmark_pos)
+
+        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
+        )
+
+        self.landmarks[landmark_id] = global_pos_3d
+
+    def get_landmark_position(self, landmark_id: str) -> 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.
+        """
+        return self.global_positions.get(landmark_id)