34 lines
1.1 KiB
Python
34 lines
1.1 KiB
Python
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) |