Occlusion Manager

nuplan/planning/script/builders/occlusion_manager_builder.py

+
+from omegaconf import DictConfig
+
+from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
+from nuplan.planning.simulation.occlusion.occlusion_manager import AbstractOcclusionManager
+from nuplan.planning.simulation.occlusion.range_occlusion_manager import RangeOcclusionManager
+
+def build_occlusion_manager(occlusion_cfg: DictConfig, scenario: AbstractScenario) -> AbstractOcclusionManager:
+    """
+    Instantiate occlusion_manager
+    :param occlusion_cfg: config of a occlusion_manager
+    :param scenario: scenario
+    :return occlusion_cfg
+    """
+    # Placeholder
+    occlusion_manager: AbstractOcclusionManager = RangeOcclusionManager(scenario)
+
+    return occlusion_manager

nuplan/planning/script/builders/simulation_builder.py

@@ -9,6 +9,7 @@ from nuplan.common.utils.distributed_scenario_filter import DistributedMode, Dis
 from nuplan.planning.scenario_builder.nuplan_db.nuplan_scenario_builder import NuPlanScenarioBuilder
 from nuplan.planning.script.builders.metric_builder import build_metrics_engines
 from nuplan.planning.script.builders.observation_builder import build_observations
+from nuplan.planning.script.builders.occlusion_manager_builder import build_occlusion_manager
 from nuplan.planning.script.builders.planner_builder import build_planners
 from nuplan.planning.script.builders.utils.utils_type import is_target_type
 from nuplan.planning.simulation.callback.abstract_callback import AbstractCallback
@@ -16,6 +17,7 @@ from nuplan.planning.simulation.callback.metric_callback import MetricCallback
 from nuplan.planning.simulation.callback.multi_callback import MultiCallback
 from nuplan.planning.simulation.controller.abstract_controller import AbstractEgoController
 from nuplan.planning.simulation.observation.abstract_observation import AbstractObservation
+from nuplan.planning.simulation.occlusion.occlusion_manager import AbstractOcclusionManager
 from nuplan.planning.simulation.planner.abstract_planner import AbstractPlanner
 from nuplan.planning.simulation.runner.simulations_runner import SimulationRunner
 from nuplan.planning.simulation.simulation import Simulation
@@ -103,6 +105,12 @@ def build_simulations(
             # Perception
             observations: AbstractObservation = build_observations(cfg.observation, scenario=scenario)
 
+            # Occlusions
+            if 'occlusion' in cfg.keys() and cfg.occlusion:
+                occlusion_manager: AbstractOcclusionManager = build_occlusion_manager(cfg.occlusion, scenario=scenario)
+            else:
+                occlusion_manager = None
+
             # Metric Engine
             metric_engine = metric_engines_map.get(scenario.scenario_type, None)
             if metric_engine is not None:
@@ -120,6 +128,7 @@ def build_simulations(
                 time_controller=simulation_time_controller,
                 observations=observations,
                 ego_controller=ego_controller,
+                occlusion_manager=occlusion_manager,
                 scenario=scenario,
             )
 

nuplan/planning/simulation/occlusion/occlusion_manager.py

+from abc import ABCMeta, abstractmethod
+from collections import deque
+from typing import Tuple
+
+from nuplan.common.actor_state.ego_state import EgoState
+from nuplan.common.actor_state.tracked_objects import TrackedObjects
+
+from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
+from nuplan.planning.simulation.history.simulation_history_buffer import SimulationHistoryBuffer
+from nuplan.planning.simulation.observation.observation_type import DetectionsTracks, Observation
+
+
+class AbstractOcclusionManager(metaclass=ABCMeta):
+    def __init__(
+        self,
+        scenario: AbstractScenario
+    ):
+        self._masks = {}
+        self.scenario = scenario
+
+    def reset(self) -> None:
+        self._masks = {}
+
+    def occlude_input(self, input_buffer: SimulationHistoryBuffer) -> SimulationHistoryBuffer:
+        ego_state_buffer = input_buffer.ego_state_buffer
+        observations_buffer = input_buffer.observation_buffer
+        sample_interval = input_buffer.sample_interval
+
+        for ego_state, observations in zip(ego_state_buffer, observations_buffer):
+            if ego_state.time_us not in self._masks:
+                self._masks[ego_state.time_us] = self._compute_mask(ego_state, observations)
+                
+        output_buffer = SimulationHistoryBuffer(ego_state_buffer, \
+                            deque([self._mask_input(ego_state.time_us, observations) for ego_state, observations in zip(ego_state_buffer, observations_buffer)]), \
+                                sample_interval)
+
+        return output_buffer
+    
+    @abstractmethod
+    def _compute_mask(self, ego_state: EgoState, observations: DetectionsTracks) -> set:
+        pass
+
+    def _mask_input(self, time_us: int, observations: DetectionsTracks) -> DetectionsTracks:
+        assert time_us in self._masks, "Attempted to mask non-cached timestep!"
+        assert isinstance(observations, DetectionsTracks), "Occlusions only support DetectionsTracks."
+
+        mask = self._masks[time_us]
+        tracks = observations.tracked_objects.tracked_objects
+
+        visible_tracks = [track for track in tracks if track.metadata.track_token in mask]
+
+        return DetectionsTracks(tracked_objects=TrackedObjects(visible_tracks))
+
+

nuplan/planning/simulation/simulation.py

@@ -45,6 +45,7 @@ class Simulation:
         self._time_controller = simulation_setup.time_controller
         self._ego_controller = simulation_setup.ego_controller
         self._observations = simulation_setup.observations
+        self._occlusion_manager = simulation_setup.occlusion_manager
         self._scenario = simulation_setup.scenario
         self._callback = MultiCallback([]) if callback is None else callback
 
@@ -136,8 +137,13 @@ class Simulation:
 
         # Extract traffic light status data
         traffic_light_data = list(self._scenario.get_traffic_light_status_at_iteration(iteration.index))
+
+        history_input = self._history_buffer
+        if self._occlusion_manager is not None:
+            history_input = self._occlusion_manager.occlude_input(history_input)
+
         logger.debug(f"Executing {iteration.index}!")
-        return PlannerInput(iteration=iteration, history=self._history_buffer, traffic_light_data=traffic_light_data)
+        return PlannerInput(iteration=iteration, history=history_input, traffic_light_data=traffic_light_data)
 
     def propagate(self, trajectory: AbstractTrajectory) -> None:
         """

nuplan/planning/simulation/simulation_setup.py

@@ -3,6 +3,7 @@ from dataclasses import dataclass
 from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
 from nuplan.planning.simulation.controller.abstract_controller import AbstractEgoController
 from nuplan.planning.simulation.observation.abstract_observation import AbstractObservation
+from nuplan.planning.simulation.occlusion.occlusion_manager import AbstractOcclusionManager
 from nuplan.planning.simulation.planner.abstract_planner import AbstractPlanner
 from nuplan.planning.simulation.simulation_time_controller.abstract_simulation_time_controller import (
     AbstractSimulationTimeController,
@@ -16,6 +17,7 @@ class SimulationSetup:
     time_controller: AbstractSimulationTimeController
     observations: AbstractObservation
     ego_controller: AbstractEgoController
+    occlusion_manager: AbstractOcclusionManager
     scenario: AbstractScenario
 
     def __post_init__(self) -> None:
@@ -30,7 +32,7 @@ class SimulationSetup:
         assert isinstance(
             self.ego_controller, AbstractEgoController
         ), 'Error: ego_controller must inherit from AbstractEgoController!'
-
+        
     def reset(self) -> None:
         """
         Reset all simulation controllers
@@ -39,6 +41,9 @@ class SimulationSetup:
         self.ego_controller.reset()
         self.time_controller.reset()
 
+        if self.occlusion_manager:
+            self.occlusion_manager.reset()
+
 
 def validate_planner_setup(setup: SimulationSetup, planner: AbstractPlanner) -> None:
     """