def test_configurable_scenario_generation_add_module_dir(self):
sink_source_dict = {
"SourceSink": [[1001.92, 1005.59], [883.064, 1009.07] ],
"Description": "merging_deu_standard",
"ConfigAgentStatesGeometries": {"Type": "UniformVehicleDistribution", "LanePositions": [0]},
"ConfigBehaviorModels": {"Type": "TestReaderFixedBehaviorType"},
"ConfigExecutionModels": {"Type": "FixedExecutionType"},
"ConfigDynamicModels": {"Type": "FixedDynamicType"},
"ConfigGoalDefinitions": {"Type": "FixedGoalTypes"},
"ConfigControlledAgents": {"Type": "RandomSingleAgent"},
"AgentParams" : {}
}
params = ParameterServer()
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["SinksSources"] = [sink_source_dict]
add_config_reader_module("bark.runtime.tests.test_config_reader_module")
scenario_generation = ConfigurableScenarioGeneration(num_scenarios=2,params=params)
scenario_generation.dump_scenario_list("test.scenario")
scenario_loader = ScenarioGeneration()
scenario_loader.load_scenario_list("test.scenario")
self.assertEqual(len(scenario_loader._scenario_list), 2)
self.assertEqual(len(scenario_loader._scenario_list[0]._agent_list), len(scenario_generation._scenario_list[0]._agent_list))
scenario = scenario_loader.get_scenario(idx=0)
# test if window is reset
scenario_generation2 = ConfigurableScenarioGeneration(num_scenarios=2,params=params)
self.assertEqual(len(scenario_generation2._scenario_list), 2)
params.Save("default_params_interaction_dataset.json")
def test_configurable_scenario_generation_interaction_merging_window(self):
track_filenames = os.path.join(os.path.dirname(__file__), "data/*_dataset_dummy_track.csv")
map_filename = os.path.join(os.path.dirname(__file__), "data/DR_DEU_Merging_MT_v01_shifted.xodr")
sink_source_dict = {
"SourceSink": [[1001.92, 1005.59], [883.064, 1009.07] ],
"Description": "merging_deu_standard",
"ConfigAgentStatesGeometries": {"Type": "InteractionDataWindowStatesGeometries", "TrackFilenames": track_filenames},
"ConfigBehaviorModels": {"Type": "InteractionDataBehaviors"},
"ConfigExecutionModels": {"Type": "FixedExecutionType"},
"ConfigDynamicModels": {"Type": "FixedDynamicType"},
"ConfigGoalDefinitions": {"Type": "FixedGoalTypes", "EnforceControlledGoal": False},
"ConfigControlledAgents": {"Type": "PositioningSingleAgent", "LanePosition" : 0},
"AgentParams" : {}
}
params = ParameterServer()
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["SinksSources"] = [sink_source_dict]
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["MapFilename"] = map_filename
scenario_generation = ConfigurableScenarioGeneration(num_scenarios=2,params=params)
scenario_generation.dump_scenario_list("test.scenario")
scenario_loader = ScenarioGeneration()
scenario_loader.load_scenario_list("test.scenario")
self.assertEqual(len(scenario_loader._scenario_list), 2)
self.assertEqual(len(scenario_loader._scenario_list[0]._agent_list), len(scenario_generation._scenario_list[0]._agent_list))
scenario = scenario_loader.get_scenario(idx=0)
# test if window is reset
scenario_generation2 = ConfigurableScenarioGeneration(num_scenarios=2,params=params)
self.assertEqual(len(scenario_generation2._scenario_list), 2)
params.Save("default_params_interaction_dataset.json")
def test_configurable_scenario_generation_interaction_merging_track_ids(self):
sink_source_dict = {
"SourceSink": [[1001.92, 1005.59], [883.064, 1009.07] ],
"Description": "merging_deu_standard",
"ConfigAgentStatesGeometries": {"Type": "InteractionDataTrackIdsStatesGeometries"},
"ConfigBehaviorModels": {"Type": "InteractionDataBehaviors"},
"ConfigExecutionModels": {"Type": "FixedExecutionType"},
"ConfigDynamicModels": {"Type": "FixedDynamicType"},
"ConfigGoalDefinitions": {"Type": "FixedGoalTypes"},
"ConfigControlledAgents": {"Type": "AgentIds", "ControlledIds" : [1]},
"AgentParams" : {}
}
params = ParameterServer()
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["SinksSources"] = [sink_source_dict]
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["MapFilename"] = \
"bark/runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.xodr"
scenario_generation = ConfigurableScenarioGeneration(num_scenarios=2,params=params)
scenario_generation.dump_scenario_list("test.scenario")
scenario_loader = ScenarioGeneration()
scenario_loader.load_scenario_list("test.scenario")
self.assertEqual(len(scenario_loader._scenario_list), 2)
self.assertEqual(len(scenario_loader._scenario_list[0]._agent_list), len(scenario_generation._scenario_list[0]._agent_list))
scenario = scenario_loader.get_scenario(idx=0)
params.Save("default_params_interaction_dataset.json")
def test_configurable_scenario_generation_parametric_observer(self):
params = ParameterServer()
mapfile = os.path.join(os.path.dirname(__file__),
"data/city_highway_straight.xodr")
conf_params = params["Scenario"]["Generation"][
"ConfigurableScenarioGeneration"]
conf_params["MapFilename"] = mapfile
conf_params["ObserverModel"] = {
"Description": "world_observer",
"ConfigObserverModel": {
"Type": "ObserverModelParametricReader"
}
}
scenario_generation = ConfigurableScenarioGeneration(num_scenarios=2,
params=params)
scenario_generation.dump_scenario_list("test.scenario")
scenario_loader = ScenarioGeneration()
scenario_loader.load_scenario_list("test.scenario")
self.assertEqual(len(scenario_loader._scenario_list), 2)
self.assertEqual(
len(scenario_loader._scenario_list[0]._agent_list),
len(scenario_generation._scenario_list[0]._agent_list))
scenario = scenario_loader.get_scenario(idx=0)
params.Save("default_params_observer.json")
def test_configurable_scenario_generation_sample_behavior_types(self):
sink_source_dict = [{
"SourceSink": [[5111.626, 5006.8305], [5110.789, 5193.1725]],
"Description": "left_lane",
"ConfigAgentStatesGeometries": {"Type": "UniformVehicleDistribution", "LanePositions": [0]},
"ConfigBehaviorModels": {"Type": "FixedBehaviorType", "ModelType": "BehaviorIDMClassic", "ModelParams": {"BehaviorIDMClassic::MaxVelocity": 60.0}},
"ConfigExecutionModels": {"Type": "FixedExecutionType"},
"ConfigDynamicModels": {"Type": "FixedDynamicType"},
"ConfigGoalDefinitions": {"Type": "FixedGoalTypes"},
"ConfigControlledAgents": {"Type": "NoneControlled"},
"AgentParams": {}
},
{
"SourceSink": [[5111.626, 5006.8305], [5110.789, 5193.1725]],
"Description": "right_lane",
"ConfigAgentStatesGeometries": {"Type": "UniformVehicleDistribution", "LanePositions": [1]},
"ConfigBehaviorModels": {"Type": "SampleBehaviorType"},
"ConfigExecutionModels": {"Type": "FixedExecutionType"},
"ConfigDynamicModels": {"Type": "FixedDynamicType"},
"ConfigGoalDefinitions": {"Type": "FixedGoalTypes"},
"ConfigControlledAgents": {"Type": "RandomSingleAgent"},
"AgentParams": {}
}]
params = ParameterServer()
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["SinksSources"] = sink_source_dict
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["MapFilename"] = os.path.join(os.path.dirname(__file__),"data/city_highway_straight.xodr")
scenario_generation = ConfigurableScenarioGeneration(
num_scenarios=2, params=params)
scenario_generation.dump_scenario_list("test.scenario")
params.Save("default_params_behavior_type_sampling.json")
def test_lane_change(self):
# World Definition
params = ParameterServer()
world = World(params)
# Model Definitions
behavior_model = BehaviorMobil(params)
execution_model = ExecutionModelInterpolate(params)
dynamic_model = SingleTrackModel(params)
behavior_model2 = BehaviorIDMLaneTracking(params)
execution_model2 = ExecutionModelInterpolate(params)
dynamic_model2 = SingleTrackModel(params)
# Map Definition
map_interface = MapInterface()
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
#agent_2d_shape = CarLimousine()
agent_2d_shape = CarRectangle()
init_state = np.array([0, 3, -1.75, 0, 5])
agent_params = params.AddChild("agent1")
goal_polygon = Polygon2d(
[1, 1, 0],
[Point2d(0, 0),
Point2d(0, 2),
Point2d(2, 2),
Point2d(2, 0)])
goal_polygon = goal_polygon.Translate(Point2d(50, -2))
agent = Agent(init_state, behavior_model, dynamic_model,
execution_model, agent_2d_shape, agent_params,
GoalDefinitionPolygon(goal_polygon), map_interface)
world.AddAgent(agent)
init_state2 = np.array([0, 15, -1.75, 0, 2])
agent2 = Agent(init_state2, behavior_model2, dynamic_model2,
execution_model2, agent_2d_shape, agent_params,
GoalDefinitionPolygon(goal_polygon), map_interface)
world.AddAgent(agent2)
# viewer
viewer = MPViewer(params=params, use_world_bounds=True)
# World Simulation
sim_step_time = params["simulation"]["step_time",
"Step-time in simulation", 0.05]
sim_real_time_factor = params["simulation"][
"real_time_factor", "execution in real-time or faster", 100]
# Draw map
for _ in range(0, 10):
viewer.clear()
world.Step(sim_step_time)
viewer.drawWorld(world)
viewer.show(block=False)
time.sleep(sim_step_time / sim_real_time_factor)
def __create_map_interface__(self):
params = ParameterServer()
# we are creating a dummy scenario to get the map interface from it
scenario = Scenario(map_file_name=self._map_file_name,
json_params=params.ConvertToDict())
world = scenario.GetWorldState()
map_interface = world.map
return map_interface
def test_dataset_scenario_generation_full_late(self):
# test wether agent 2 coming in late is correctly identified as invalid at first world time step
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track_late.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["StartingOffsetMs"] = 0
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=1)
scenario = scenario_generation.get_scenario(0)
world_state = scenario.GetWorldState()
agent1 = world_state.GetAgent(1)
agent2 = world_state.GetAgent(2)
self.assertAlmostEqual(agent1.first_valid_timestamp, 0.0)
self.assertAlmostEqual(agent2.first_valid_timestamp, 0.3)
self.assertEqual(isinstance(agent1, Agent), True)
self.assertEqual(agent1.IsValidAtTime(world_state.time), True)
self.assertEqual(isinstance(agent2, Agent), True)
self.assertEqual(agent2.IsValidAtTime(world_state.time), False)
def test_setting_behavior_of_ego_agent(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
# Set behaviour model of the ego agent
params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]["BehaviorModel"] = \
{"ego": "BehaviorMobilRuleBased"}
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=2)
for scenario, _ in scenario_generation:
ego_id = scenario.eval_agent_ids[
0] # Assume there is only one ego agent
for agent in scenario._agent_list:
if agent.id == ego_id:
behavior_model = agent.behavior_model
self.assertEqual(
str(behavior_model).rsplit(".")[-1],
"BehaviorMobilRuleBased")
break
else:
# No ego agent in scenario._agent_list, something is wrong
self.assertTrue(False)
def test_excluded_tracks(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename_1 = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track.csv")
track_filename_2 = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track_2.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename_1, track_filename_2]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["ExcludeTracks"] = {
track_filename_1: [2],
track_filename_2: [1, 3],
}
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=10)
for scenario, _ in scenario_generation:
track_filename = scenario.json_params["track_file"]
ego_id = scenario.eval_agent_ids[0]
# Check that pair (track_filename, ego_id) is NOT in ExcludeTracks
self.assertNotIn((track_filename, ego_id), [(track_filename_1, 2),
(track_filename_2, 1),
(track_filename_2, 3)])
def test_dataset_scenario_generation_overwrite_behavior_model(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_DEU_Merging_dummy_track.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"][
"TrackFilename"] = track_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["TrackIds"] = [1, 2]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["StartTs"] = 500
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["EndTs"] = 1000
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["EgoTrackId"] = 1
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["BehaviorModel"] = {
"2": "BehaviorMobilRuleBased"
}
scenario_generation = InteractionDatasetScenarioGeneration(
params=params, num_scenarios=1)
beh_model = scenario_generation.get_scenario(
0)._agent_list[1].behavior_model
self.assertTrue(isinstance(beh_model, BehaviorMobilRuleBased))
def test_dataset_scenario_generation(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"][
"TrackFilename"] = track_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["TrackIds"] = [1, 2]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["StartTs"] = 500
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["EndTs"] = 1000
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGeneration"]["EgoTrackId"] = 1
scenario_generation = InteractionDatasetScenarioGeneration(
params=params, num_scenarios=1)
self.assertEqual(scenario_generation.get_num_scenarios(), 1)
def test_configurable_scenario_generation_default_params(self):
params = ParameterServer()
mapfile = os.path.join(os.path.dirname(__file__),"data/city_highway_straight.xodr")
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"]["MapFilename"] = mapfile
scenario_generation = ConfigurableScenarioGeneration(
num_scenarios=2, params=params)
scenario_generation.dump_scenario_list("test.scenario")
scenario_loader = ScenarioGeneration()
scenario_loader.load_scenario_list("test.scenario")
self.assertEqual(len(scenario_loader._scenario_list), 2)
self.assertEqual(len(scenario_loader._scenario_list[0]._agent_list), len(
scenario_generation._scenario_list[0]._agent_list))
scenario = scenario_loader.get_scenario(idx=0)
params.Save("default_params.json")
def __create_single_scenario__(self, scen_track_info):
scen_track_info.TimeSanityCheck()
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.ConvertToDict(),
map_interface=self._map_interface)
world = scenario.GetWorldState()
track_params = ParameterServer()
track_params["execution_model"] = 'ExecutionModelInterpolate'
track_params["dynamic_model"] = 'SingleTrackModel'
track_params["map_interface"] = world.map
all_track_ids = list(scen_track_info.GetOtherTrackInfos().keys())
# also add ego id
ego_track_id = scen_track_info.GetEgoTrackInfo().GetTrackId()
all_track_ids.append(ego_track_id)
agent_list = []
model_converter = ModelJsonConversion()
for track_id in all_track_ids:
if self._behavior_model and track_id != ego_track_id:
behavior_params = self.__fill_agent_params()
behavior_model_name = self._behavior_model
track_params["behavior_model"] = model_converter.convert_model(
behavior_model_name, behavior_params)
# behavior_params.Save("/tmp/agent_prams_{}.json".format(track_id))
else:
# we do not change behavior model of ego agent -> will be set in benchmark
track_params["behavior_model"] = None
if self._use_goal_from_road:
goal = self.__infer_goal_from_road__(lc=0)
else:
goal = None
agent = self._interaction_ds_reader.AgentFromTrackfile(
track_params,
self._params,
scen_track_info,
track_id,
goal_def=goal)
# set first valid time stamp of the agent (in relation to scenario start)
agent.first_valid_timestamp = scen_track_info.GetTimeOffsetOfAgentInSec(
track_id)
agent_list.append(agent)
scenario._agent_list = agent_list # must contain all agents!
scenario._eval_agent_ids = [ego_track_id]
scenario.json_params["track_file"] = scen_track_info.GetTrackFilename()
return scenario
def test_world(self):
# create agent
params = ParameterServer()
behavior = BehaviorConstantAcceleration(params)
execution = ExecutionModelInterpolate(params)
dynamic = SingleTrackModel(params)
shape = Polygon2d([1.25, 1, 0], [
Point2d(0, 0),
Point2d(0, 2),
Point2d(4, 2),
Point2d(4, 0),
Point2d(0, 0)
])
init_state = np.array([0, 0, 0, 0, 5])
agent = Agent(init_state, behavior, dynamic, execution, shape,
params.AddChild("agent"))
road_map = OpenDriveMap()
newXodrRoad = XodrRoad()
newXodrRoad.id = 1
newXodrRoad.name = "Autobahn A9"
newPlanView = PlanView()
newPlanView.AddLine(Point2d(0, 0), 1.57079632679, 10)
newXodrRoad.plan_view = newPlanView
line = newXodrRoad.plan_view.GetReferenceLine().ToArray()
p = Point2d(line[-1][0], line[-1][1])
newXodrRoad.plan_view.AddSpiral(p, 1.57079632679, 50.0, 0.0, 0.3, 0.4)
line = newXodrRoad.plan_view.GetReferenceLine()
lane_section = XodrLaneSection(0)
lane = XodrLane()
lane.line = line
lane_section.AddLane(lane)
newXodrRoad.AddLaneSection(lane_section)
road_map.AddRoad(newXodrRoad)
r = Roadgraph()
map_interface = MapInterface()
map_interface.SetOpenDriveMap(road_map)
map_interface.SetRoadgraph(r)
world = World(params)
world.AddAgent(agent)
def __create_single_scenario__(self, scenario_track_info):
scenario_track_info.TimeSanityCheck()
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.ConvertToDict())
# as we always use the same world, we can create the MapIntf. once
if self._map_interface is None:
scenario.CreateMapInterface(self._map_file_name)
print("Creating New Map Interface for Scenario!")
else:
scenario.map_interface = self._map_interface
self._map_interface = scenario.map_interface
world = scenario.GetWorldState()
track_params = ParameterServer()
track_params["execution_model"] = 'ExecutionModelInterpolate'
track_params["dynamic_model"] = 'SingleTrackModel'
track_params["map_interface"] = world.map
all_track_ids = list(scenario_track_info.GetOtherTrackInfos().keys())
# also add ego id
ego_track_id = scenario_track_info.GetEgoTrackInfo().GetTrackId()
all_track_ids.append(ego_track_id)
agent_list = []
model_converter = ModelJsonConversion()
for track_id in all_track_ids:
if self._behavior_model and track_id != ego_track_id:
behavior_params = self.__fill_agent_params(
scenario_track_info.GetEgoTrackInfo(),
scenario_track_info.GetOtherTrackInfos()[track_id])
behavior_model_name = self._behavior_model
track_params["behavior_model"] = model_converter.convert_model(
behavior_model_name, behavior_params)
# behavior_params.Save("/tmp/agent_prams_{}.json".format(track_id))
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, track_id)
agent.first_valid_timestamp = scenario_track_info.GetOffsetOfAgentMillisec(
track_id)
agent_list.append(agent)
scenario._agent_list = agent_list # must contain all agents!
scenario._eval_agent_ids = [
scenario_track_info.GetEgoTrackInfo().GetTrackId()
]
scenario.json_params[
"track_file"] = scenario_track_info.GetTrackFilename()
return scenario
def test_dataset_scenario_generation_full(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__), "data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(os.path.dirname(__file__), "data/interaction_dataset_dummy_track.csv")
params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]["MapFilename"] = map_filename
params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]["TrackFilenameList"] = [track_filename]
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=2)
self.assertEqual(scenario_generation.get_num_scenarios(), 2)
def test_dataset_scenario_generation_full(self):
"""
Checking Track file with two track ids, both become valid at time 0.
Two Scenarios should be created from this.
"""
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_DEU_Merging_dummy_track.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["StartingOffsetMs"] = 0
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=2)
self.assertEqual(scenario_generation.get_num_scenarios(), 2)
# first scenario
scenario0 = scenario_generation.get_scenario(0)
agent11 = scenario0.GetWorldState().agents[1]
agent12 = scenario0.GetWorldState().agents[2]
self.assertEqual(agent11.first_valid_timestamp, 0.0)
self.assertEqual(agent12.first_valid_timestamp, 0.0)
# agents are initialized with Behavior::NotValidYet
self.assertEqual(list(scenario0.GetWorldState().agents_valid.keys()),
[])
scenario0.GetWorldState().Step(0.01)
self.assertEqual(list(scenario0.GetWorldState().agents_valid.keys()),
[1, 2])
# second scenario
scenario1 = scenario_generation.get_scenario(1)
agent21 = scenario1.GetWorldState().agents[1]
agent22 = scenario1.GetWorldState().agents[2]
self.assertEqual(agent21.first_valid_timestamp, 0.0)
self.assertEqual(agent22.first_valid_timestamp, 0.0)
self.assertEqual(list(scenario1.GetWorldState().agents_valid.keys()),
[])
scenario1.GetWorldState().Step(0.01)
self.assertEqual(list(scenario1.GetWorldState().agents_valid.keys()),
[1, 2])
def test_dataset_scenario_generation_full_outside3(self):
"""
Checking Track file with three track ids. test wether agent 3 (the ego agent
of the scenario) outside at the beginning is correctly identified as valid at first
world time step (although in the track files, it becomes valid later than the other agents
-> will be cut off for scenario)
"""
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_DEU_Merging_dummy_track_outside.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["StartingOffsetMs"] = 0
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=3)
scenario = scenario_generation.get_scenario(2)
self.assertAlmostEqual(scenario.eval_agent_ids, [3])
world_state = scenario.GetWorldState()
agent31 = world_state.GetAgent(1)
agent32 = world_state.GetAgent(2)
agent33 = world_state.GetAgent(3)
# they all should be valid at the beginning
world_state.time = 0
self.assertEqual(isinstance(agent31, Agent), True)
self.assertEqual(agent31.IsValidAtTime(world_state.time), True)
self.assertEqual(agent31.InsideRoadCorridor(), True)
self.assertEqual(isinstance(agent32, Agent), True)
self.assertEqual(agent32.IsValidAtTime(world_state.time), True)
self.assertEqual(agent32.InsideRoadCorridor(), True)
self.assertEqual(isinstance(agent33, Agent), True)
self.assertEqual(agent33.IsValidAtTime(world_state.time), True)
self.assertEqual(agent33.InsideRoadCorridor(), True)
world_state.Step(0.05)
self.assertEqual(len(world_state.agents_valid), 3)
def __create_single_scenario__(self, scenario_track_info):
scenario_track_info.TimeSanityCheck()
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.ConvertToDict())
# as we always use the same world, we can create the MapIntf. once
if self._map_interface is None:
scenario.CreateMapInterface(self._map_file_name)
print("Creating New Map Interface for Scenario!")
else:
scenario.map_interface = self._map_interface
self._map_interface = scenario.map_interface
world = scenario.GetWorldState()
agent_list = []
track_params = ParameterServer()
track_params["execution_model"] = 'ExecutionModelInterpolate'
track_params["dynamic_model"] = 'SingleTrackModel'
track_params["map_interface"] = world.map
for id_other in scenario_track_info.GetOtherTrackInfos().keys():
if str(id_other) in self._behavior_models:
track_params["behavior_model"] = self._behavior_models[str(
id_other)]
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, id_other)
agent_list.append(agent)
id_ego = scenario_track_info.GetEgoTrackInfo().GetTrackId()
if "ego" in self._behavior_models:
# Always set this behavior to the ego agent, regardless of agent's ID
track_params["behavior_model"] = self._behavior_models["ego"]
elif str(id_ego) in self._behavior_models:
track_params["behavior_model"] = self._behavior_models[str(id_ego)]
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, id_ego)
agent_list.append(agent)
scenario._agent_list = agent_list # must contain all agents!
scenario._eval_agent_ids = [
scenario_track_info.GetEgoTrackInfo().GetTrackId()
]
scenario.json_params[
"track_file"] = scenario_track_info.GetTrackFilename()
return scenario
def default_agent_model(self):
param_server = ParameterServer()
behavior_model = BehaviorConstantAcceleration(param_server)
execution_model = ExecutionModelInterpolate(param_server)
dynamic_model = SingleTrackModel(param_server)
map_interface = MapInterface()
agent_2d_shape = CarLimousine()
init_state = np.array([0, 0, 0, 0, 0])
agent_default = Agent(init_state, behavior_model, dynamic_model,
execution_model, agent_2d_shape, param_server)
return agent_default
def test_dataset_scenario_generation_full_incomplete(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__), "data/DR_CHN_Merging_ZS_partial_v02.xodr")
track_filename = os.path.join(os.path.dirname(__file__), "data/interaction_dataset_dummy_track_incomplete.csv")
params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]["MapFilename"] = map_filename
params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]["TrackFilenameList"] = [track_filename]
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=3)
# agent 1 is not part of the map, so it should only generate 2 scenarios
self.assertEqual(scenario_generation.get_num_scenarios(), 2)
def test_included_tracks(self):
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename_1 = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track.csv")
track_filename_2 = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_dummy_track_2.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename_1, track_filename_2]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["IncludeTracks"] = {
track_filename_1: [2],
track_filename_2: [1, 3],
}
def assert_correct_combinations(scenario_generation):
for scenario, _ in scenario_generation:
track_filename = scenario.json_params["track_file"]
ego_id = scenario.eval_agent_ids[0]
# Check if pair (track_filename, ego_id) is in IncludeTracks
self.assertIn((track_filename, ego_id),
[(track_filename_1, 2), (track_filename_2, 1),
(track_filename_2, 3)])
# CASE 1: num_scenarios < number of scenarios in IncludeTracks
scenario_generation_1 = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=2)
# Only two scenarios should be generated (num_scenarios=2)
self.assertEqual(scenario_generation_1.get_num_scenarios(), 2)
assert_correct_combinations(scenario_generation_1)
# CASE 2: num_scenarios > number of scenarios in IncludeTracks
scenario_generation_2 = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=4)
# Three scenarios should be generated (all specified in IncludeTracks)
self.assertEqual(scenario_generation_2.get_num_scenarios(), 3)
assert_correct_combinations(scenario_generation_2)
def default_drone_model(self):
param_server = ParameterServer()
dynamic_model = SingleTrackModel(param_server)
behavior_model = BehaviorConstantAcceleration(param_server)
execution_model = ExecutionModelInterpolate(param_server)
drone_2d_shape = Polygon2d([0, 0, 0],
np.array([[0.3, 0.1], [0.3, -0.1],
[0.1, -0.3], [-0.1, -0.3],
[-0.3, -0.1], [-0.3, 0.1],
[-0.1, 0.3], [0.1, 0.3],
[0.3, 0.1]]))
init_state = np.array([0, 0, 0, 0, 0])
drone_default = Agent(init_state, behavior_model, dynamic_model,
execution_model, drone_2d_shape, param_server)
return drone_default
def initialize_params(self, params):
super().initialize_params(params)
params_temp = \
self._params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]
self._map_file_name = params_temp[
"MapFilename", "Path to the open drive map",
"bark/runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.xodr"]
self._track_file_name_list = params_temp[
"TrackFilenameList", "List of Paths to track files (csv)", [
"bark/runtime/tests/data/interaction_dataset_DEU_Merging_dummy_track.csv"
]]
self._behavior_model = params_temp[
"BehaviorModel",
"Overwrite static trajectory of others with behavior model", None]
self._xy_offset = params_temp["XYOffset",
"offset in x and y direction.", [0, 0]]
self._excluded_tracks = params_temp[
"ExcludeTracks",
"Track IDs to be excluded from the scenario generation", []]
self._base_params_json = params_temp[
"BaseParams", "Initial parameters of each scenario",
ParameterServer(log_if_default=True)].ConvertToDict()
self._agent_params = []
self._starting_offset_ms = params_temp[
"StartingOffsetMs", "Starting Offset to each agent in miliseconds",
500]
self._road_ids = params_temp["RoadIds", "Road ids for road corridor.",
[0, 1]]
self._vehicle_length_max = params_temp[
"VehicleLengthMax", "Maximum allowed vehicle length", 5.0]
self._use_shape_from_track = params_temp["UseShapeFromTrack",
"Use shape from track", True]
self._use_rectangle_shape = params_temp["RectangleShape",
"Use Rectangle vehicle shape",
True]
self._use_goal_from_road = params_temp["GoalFromRoad",
"Use goal from road", False]
self._rel_pose_goal_on_road = params_temp[
"RelPoseGoalOnRoad", "Relative position of goal on road", 0.99]
self._interaction_ds_reader = InteractionDatasetReader(
use_shape_from_track=self._use_shape_from_track,
use_rectangle_shape=self._use_rectangle_shape)
def initialize_params(self, params):
super().initialize_params(params)
params_temp = \
self._params["Scenario"]["Generation"]["InteractionDatasetScenarioGenerationFull"]
self._map_file_name = params_temp["MapFilename",
"Path to the open drive map",
"bark/runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.xodr"]
self._track_file_name_list = params_temp["TrackFilenameList",
"List of Paths to track files (csv)",
["bark/runtime/tests/data/interaction_dataset_dummy_track.csv"]]
self._behavior_model = params_temp["BehaviorModel",
"Overwrite static trajectory with behavior model", None]
self._xy_offset = params_temp["XYOffset",
"offset in x and y direction.", [0, 0]]
self._excluded_tracks = params_temp[
"ExcludeTracks", "Track IDs to be excluded from the scenario generation", []]
self._base_params_json = params_temp[
"BaseParams", "Initial parameters of each scenario", ParameterServer(log_if_default=True)].ConvertToDict()
self._agent_params = []
self._starting_offset_ms = params_temp["StartingOffsetMs",
"Starting Offset to each agent in miliseconds", 500]
def __create_single_scenario__(self, scenario_track_info):
scenario_track_info.TimeSanityCheck()
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.ConvertToDict())
world = scenario.GetWorldState()
track_params = ParameterServer()
track_params["execution_model"] = 'ExecutionModelInterpolate'
track_params["dynamic_model"] = 'SingleTrackModel'
track_params["map_interface"] = world.map
all_track_ids = list(scenario_track_info.GetOtherTrackInfos().keys())
# also add ego id
all_track_ids.append(
scenario_track_info.GetEgoTrackInfo().GetTrackId())
agent_list = []
model_converter = ModelJsonConversion()
for track_id in all_track_ids:
if str(track_id) in self._behavior_models:
behavior_params = self.__fill_agent_params__()
behavior_model_name = self._behavior_models[str(track_id)]
track_params["behavior_model"] = model_converter.convert_model(
behavior_model_name, behavior_params)
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, track_id)
# agent_params.Save("/tmp/agent_params_{}.json".format(track_id))
agent_list.append(agent)
scenario._agent_list = agent_list # must contain all agents!
scenario._eval_agent_ids = [
scenario_track_info.GetEgoTrackInfo().GetTrackId()
]
return scenario
def __create_single_scenario__(self, scenario_track_info):
scenario_track_info.TimeSanityCheck()
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.ConvertToDict())
world = scenario.GetWorldState()
agent_list = []
track_params = ParameterServer()
track_params["execution_model"] = 'ExecutionModelInterpolate'
track_params["dynamic_model"] = 'SingleTrackModel'
track_params["map_interface"] = world.map
for id_other in scenario_track_info.GetOtherTrackInfos().keys():
if str(id_other) in self._behavior_models:
track_params["behavior_model"] = self._behavior_models[str(
id_other)]
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, id_other)
agent_list.append(agent)
id_ego = scenario_track_info.GetEgoTrackInfo().GetTrackId()
if str(id_ego) in self._behavior_models:
track_params["behavior_model"] = self._behavior_models[str(id_ego)]
else:
track_params["behavior_model"] = None
agent = self.interaction_ds_reader.AgentFromTrackfile(
track_params, self._params, scenario_track_info, id_ego)
agent_list.append(agent)
scenario._agent_list = agent_list # must contain all agents!
scenario._eval_agent_ids = [
scenario_track_info.GetEgoTrackInfo().GetTrackId()
]
return scenario
def test_dataset_scenario_generation_full_incomplete(self):
"""
Checking Track file with three track ids, but agent 1 is never inside the map,
so it should only generate 2 scenarios
"""
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_CHN_Merging_ZS_partial_v02.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_CHN_Merging_dummy_track_incomplete.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["StartingOffsetMs"] = 0
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=3)
self.assertEqual(scenario_generation.get_num_scenarios(), 2)
# first scenario
agent12 = scenario_generation.get_scenario(0).GetWorldState().agents[2]
agent17 = scenario_generation.get_scenario(0).GetWorldState().agents[7]
self.assertEqual(agent12.first_valid_timestamp, 0.0)
self.assertEqual(agent17.first_valid_timestamp, 0.0)
# second scenario
agent22 = scenario_generation.get_scenario(1).GetWorldState().agents[2]
agent27 = scenario_generation.get_scenario(1).GetWorldState().agents[7]
self.assertEqual(agent22.first_valid_timestamp, 0.0)
self.assertEqual(agent27.first_valid_timestamp, 0.0)
def test_dataset_scenario_generation_full_outside3_behavior_overwritten(
self):
"""
Checking Track file with three track ids. test wether agent 3 (the ego agent
of the scenario) outside at the beginning is correctly identified as valid at first
world time step (although in the track files, it becomes valid later than the other agents
-> will be cut off for scenario). Overwriting Behavior Model.
"""
params = ParameterServer()
map_filename = os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr")
track_filename = os.path.join(
os.path.dirname(__file__),
"data/interaction_dataset_DEU_Merging_dummy_track_outside.csv")
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"MapFilename"] = map_filename
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"TrackFilenameList"] = [track_filename]
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"]["StartingOffsetMs"] = 0
params["Scenario"]["Generation"][
"InteractionDatasetScenarioGenerationFull"][
"BehaviorModel"] = "BehaviorMobilRuleBased"
scenario_generation = InteractionDatasetScenarioGenerationFull(
params=params, num_scenarios=3)
scenario = scenario_generation.get_scenario(2)
self.assertAlmostEqual(scenario.eval_agent_ids, [3])
world_state = scenario.GetWorldState()
agent31 = world_state.GetAgent(1)
agent32 = world_state.GetAgent(2)
agent33 = world_state.GetAgent(3)
# others are defined as BehaviorMobilRuleBased
self.assertTrue(
isinstance(agent31.behavior_model, BehaviorMobilRuleBased))
self.assertTrue(
isinstance(agent32.behavior_model, BehaviorMobilRuleBased))
self.assertTrue(
isinstance(agent33.behavior_model, BehaviorStaticTrajectory))
# they all should be valid at the beginning
world_state.time = 0
self.assertEqual(isinstance(agent31, Agent), True)
self.assertEqual(agent31.IsValidAtTime(world_state.time), True)
self.assertEqual(agent31.InsideRoadCorridor(), True)
self.assertEqual(isinstance(agent32, Agent), True)
self.assertEqual(agent32.IsValidAtTime(world_state.time), True)
self.assertEqual(agent32.InsideRoadCorridor(), True)
self.assertEqual(isinstance(agent33, Agent), True)
self.assertEqual(agent33.IsValidAtTime(world_state.time), True)
self.assertEqual(agent33.InsideRoadCorridor(), True)
self.assertEqual(isinstance(agent33, Agent), True)
self.assertEqual(agent33.IsValidAtTime(world_state.time), True)
self.assertEqual(agent33.InsideRoadCorridor(), True)
evaluator = EvaluatorCollisionAgents()
world_state.AddEvaluator("collision", evaluator)
info = world_state.Evaluate()
self.assertEqual(info["collision"], False)
world_state.Step(0.05)
evaluator = EvaluatorCollisionAgents()
world_state.AddEvaluator("collision", evaluator)
info = world_state.Evaluate()
self.assertEqual(info["collision"], False)
self.assertEqual(len(world_state.agents_valid), 3)