Skip to content
Snippets Groups Projects
Commit 7605e30b authored by Eli Henry Dykhne's avatar Eli Henry Dykhne
Browse files

sequential injection set up and removal code malfunctioning

parent a696548e
No related branches found
No related tags found
2 merge requests!21Major changes. convert nuplan devkit to handle occlusions for project,!19Single class generation and metrics updates
This diff is collapsed.
......@@ -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))
......
......@@ -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
......
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:
......
......@@ -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:
......
......@@ -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)
......
......@@ -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()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment