sequential injection set up and removal code malfunctioning

nuplan/planning/scenario_builder/scenario_modifier/conflict_vehicle_injection_and_occlusion_injection.py

@@ -45,9 +45,6 @@ class ConflictInjectionAndOcclusionInjectionModifier(OcclusionInjectionModifier)
         
     def modify_scenario(self, runner: SimulationRunner) -> List[SimulationRunner]:
         scenario = runner.scenario
-        # determine if ego moves through a lane connector
-        # determine what kind of intersection transition we make
-        # compute potential conflict lane connectors
         
         crossing_lane_connector = self.how_does_ego_cross_intersection(runner)
         if crossing_lane_connector is None:
@@ -154,7 +151,7 @@ class ConflictInjectionAndOcclusionInjectionModifier(OcclusionInjectionModifier)
                 goal_point = Point2D(last.x, last.y)
                 potential_conflict_vehicle_goal = goal_segment.baseline_path.get_nearest_pose_from_position(goal_point)
         
-                potential_conflict_vehicle_speed = ego_agent.velocity.magnitude() #we will use the same speed as the ego, so hopefully the oncoming vehicle will arive at the conflict point at the same time
+                potential_conflict_vehicle_speed = conflict_vehicle_speed
                 potential_conflict_vehicle_heading = potential_conflict_vehicle_spawn_pose.heading
                 potential_conflict_vehicle_velocity = StateVector2D(potential_conflict_vehicle_speed * math.cos(potential_conflict_vehicle_heading), potential_conflict_vehicle_speed * math.sin(potential_conflict_vehicle_heading))
         

nuplan/planning/scenario_builder/scenario_modifier/occlusion_injection_modifier.py

@@ -526,12 +526,12 @@ class OcclusionInjectionModifier(AbstractScenarioModifier):
         ]
     )
     
-    def get_map_geometry(self, ego_object: AgentState, map_api: AbstractMap, traffic_light_status: Dict[TrafficLightStatusType, List[str]], lane_objects_to_prune_by = List[LaneGraphEdgeMapObject]) -> Tuple[MultiLineString, MultiPolygon]:
+    def get_map_geometry(self, ego_object: AgentState, map_api: AbstractMap, traffic_light_status: Dict[TrafficLightStatusType, List[str]], lane_objects_to_prune_by: List[LaneGraphEdgeMapObject] = None) -> Tuple[MultiLineString, MultiPolygon]:
         """Helper function to get map geometry from a map.
         :param ego_object: ego object to center map on
         :param map_api: what it says on the tin
         :param traffic_light_status: traffic light status at the time of injection
-        :param ids_to_prune_by: list of laneobjectd that must be matched to if we want to keep a particular lane centerline. lane connectors must match exactly and lanes must have their parent roadblock match to allow for occlusions by cars in adjacent lanes
+        :param ids_to_prune_by: list of laneobjects that must be matched to if we want to keep a particular lane centerline. lane connectors must match exactly and lanes must have their parent roadblock match to allow for occlusions by cars in adjacent lanes. if it is None, we do not prune
         :return: A multilinestring of all the centerlines, and a multipolygon of all the map polygons
         """
         
@@ -542,11 +542,10 @@ class OcclusionInjectionModifier(AbstractScenarioModifier):
         map_polys = []
         for layer in layers:
             for obj in map_object_dict[layer]:
-                if isinstance(obj, LaneConnector):
-                    if (obj.id not in [lane_object.id for lane_object in lane_objects_to_prune_by]):
+                if lane_objects_to_prune_by is not None:
+                    if isinstance(obj, LaneConnector) and (obj.id not in [lane_object.id for lane_object in lane_objects_to_prune_by]):
                         continue
-                else: #if its a lane
-                    if (obj.parent.id not in [lane_object.parent.id for lane_object in lane_objects_to_prune_by]): #parent block must match
+                    elif (obj.parent.id not in [lane_object.parent.id for lane_object in lane_objects_to_prune_by]): #parent block must match
                         continue
                 if (obj.id not in traffic_light_status[TrafficLightStatusType.RED]):
                     centerlines.append(obj.baseline_path.linestring)
@@ -565,11 +564,17 @@ class OcclusionInjectionModifier(AbstractScenarioModifier):
         """
         agents = [ego_agent] + other_agents
         steps = int(horizon / interval)
+        # print(len(agents))
+        # print(steps)
+        # for agent in agents:
+        #     print(agent.center.x, agent.center.y, agent.velocity.x, agent.velocity.y)
+        #     print(agent.center.x, agent.center.y, agent.velocity.magnitude() * math.cos(agent.center.heading), agent.velocity.magnitude() * math.sin(agent.center.heading))
+
         for step in range(1, steps):
             for agent1, agent2 in itertools.combinations(agents, 2):
                 time = step * interval
-                curr_poly1 = affinity.translate(agent1.box.geometry, xoff=agent1.velocity.x * time, yoff=agent1.velocity.y * time)
-                curr_poly2 = affinity.translate(agent2.box.geometry, xoff=agent2.velocity.x * time, yoff=agent2.velocity.y * time)
+                curr_poly1 = affinity.translate(agent1.box.geometry, xoff=agent1.velocity.magnitude() * math.cos(agent1.center.heading) * time, yoff=agent1.velocity.magnitude() * math.sin(agent1.center.heading) * time)
+                curr_poly2 = affinity.translate(agent2.box.geometry, xoff=agent2.velocity.magnitude() * math.cos(agent2.center.heading) * time, yoff=agent2.velocity.magnitude() * math.sin(agent2.center.heading) * time)
                 if curr_poly1.intersects(curr_poly2):
                     return time
         return None

nuplan/planning/script/builders/scenario_modifier_builder.py

 from typing import List
 
+from omegaconf import DictConfig
+
 from nuplan.planning.scenario_builder.scenario_modifier.abstract_scenario_modifier import AbstractScenarioModifier
 from nuplan.planning.scenario_builder.scenario_modifier.left_and_right_modifier import LeftAndRightModifier
 from nuplan.planning.scenario_builder.scenario_modifier.occlusion_injection_modifier import OcclusionInjectionModifier
 from nuplan.planning.scenario_builder.scenario_modifier.oncoming_vehicle_injection_for_left_turn_and_occlusion_injection import OncomingInjectionForLeftTurnAndOcclusionInjectionModifier
 from nuplan.planning.scenario_builder.scenario_modifier.conflict_vehicle_injection_and_occlusion_injection import ConflictInjectionAndOcclusionInjectionModifier
+from nuplan.planning.scenario_builder.scenario_modifier.sequential_conflict_with_occlusion_injection_modifier import SequentialConflictWithOcclusionInjectionModifier
 
 
-def build_scenario_modifiers(scenario_modifier_types: List[str]) -> List[AbstractScenarioModifier]:
+def build_scenario_modifiers(scenario_modifier_types: List[str], cfg: DictConfig = None) -> List[AbstractScenarioModifier]:
     modifiers = []
     for type in scenario_modifier_types:
         if type == "left-and-right":
@@ -18,6 +21,8 @@ def build_scenario_modifiers(scenario_modifier_types: List[str]) -> List[Abstrac
             modifiers.append(OncomingInjectionForLeftTurnAndOcclusionInjectionModifier())
         elif type == "conflict-and-occlusion-injection":
             modifiers.append(ConflictInjectionAndOcclusionInjectionModifier())
+        elif type == "sequential-conflict-with-occlusion-injection":
+            modifiers.append(SequentialConflictWithOcclusionInjectionModifier(cfg))
         # elif type == "occludie-injection":
         #     modifiers.append(OccludieInjection())
         else:

nuplan/planning/script/builders/simulation_builder.py

@@ -211,7 +211,7 @@ def modify_simulations(simulation: SimulationRunner, cfg: DictConfig) -> Tuple[s
     :return: original simulation token, log message, and list of modifications
     """
     modifier_types = cfg.modifier_types
-    scenario_modifiers = build_scenario_modifiers(modifier_types)
+    scenario_modifiers = build_scenario_modifiers(modifier_types, cfg)
     all_modified_simulations = []
     log = ''
     for modifier in scenario_modifiers:

nuplan/planning/simulation/observation/ml_planner_agents.py

@@ -310,10 +310,39 @@ class MLPlannerAgents(AbstractObservation):
 
         return output_buffer
     
+    def remove_all_of_object_types_from_scene(self, agent_types: List[TrackedObjectType], simulation: Simulation):
+        """Removes all agents of a given type from the scene
+        """
+        g = len(simulation._observations._get_agents().keys())
+        print('lenobj', len(simulation._observations.get_observation().tracked_objects.tracked_objects))
+        for i, obj in enumerate(simulation._observations.get_observation().tracked_objects.tracked_objects):
+            print(i, obj.tracked_object_type)
+            if obj.tracked_object_type in agent_types:
+                simulation._observations.remove_agent_from_scene(obj, simulation)
+            print('lenobj', i, len(simulation._observations.get_observation().tracked_objects.tracked_objects))
+                
+        
+        h = len(simulation._observations._get_agents().keys())
+        print(simulation.scenario.token, 'g, h', g, h)
+    
     def remove_agent_from_scene(self, agent: Agent, simulation: Simulation):
         """Removes an agent from the scene
         """
-        simulation._observations._get_agents().pop(agent.metadata.track_token)
+        if simulation._history_buffer is None:
+            simulation._history_buffer = SimulationHistoryBuffer.initialize_from_scenario(
+                simulation._history_buffer_size, simulation._scenario, simulation._observations.observation_type()
+            )
+        
+        if agent.metadata.track_token in simulation._observations._get_agents():
+            c = len(simulation._observations._get_agents().keys())
+            simulation._observations._get_agents().pop(agent.metadata.track_token)
+            d = len(simulation._observations._get_agents().keys())
+            print(simulation.scenario.token, 'c, d', c, d)
+
+        e = len(simulation._observations._static_agents)
+        simulation._observations._static_agents = [a for a in simulation._observations._static_agents if a.metadata.track_token != agent.metadata.track_token]
+        f = len(simulation._observations._static_agents)
+        print(simulation.scenario.token, 'e, f', e, f)
         history_buffer = simulation._history_buffer
         new_observation_buffer = deque()
         for observations in history_buffer.observation_buffer:
@@ -324,7 +353,9 @@ class MLPlannerAgents(AbstractObservation):
 
             new_observation_buffer.append(DetectionsTracks(TrackedObjects(tracks)))
             
-        simulation._observations._ego_state_history.pop(agent.metadata.track_token)
+        if agent.metadata.track_token in simulation._observations._ego_state_history:
+            #print('hi2')
+            simulation._observations._ego_state_history.pop(agent.metadata.track_token)
         
         simulation._history_buffer = SimulationHistoryBuffer(history_buffer.ego_state_buffer, new_observation_buffer, history_buffer.sample_interval)
 

nuplan/planning/simulation/simulation.py

@@ -112,9 +112,10 @@ class Simulation:
         self.reset()
 
         # Initialize history from scenario
-        self._history_buffer = SimulationHistoryBuffer.initialize_from_scenario(
-            self._history_buffer_size, self._scenario, self._observations.observation_type()
-        )
+        if self._history_buffer is None:
+            self._history_buffer = SimulationHistoryBuffer.initialize_from_scenario(
+                self._history_buffer_size, self._scenario, self._observations.observation_type()
+            )
 
         # Initialize observations
         self._observations.initialize()