def test_database_multiprocessing_history(self):
dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=2)
dbs.process("data/database1")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent",
"max_steps": "EvaluatorStepCount"}
terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2}
params = ParameterServer() # only for evaluated agents not passed to scenario!
behaviors_tested = {"IDM": BehaviorIDMClassic(params), "Const" : BehaviorConstantAcceleration(params)}
benchmark_runner = BenchmarkRunnerMP(benchmark_database=db,
evaluators=evaluators,
terminal_when=terminal_when,
behaviors=behaviors_tested,
log_eval_avg_every=10)
rst = benchmark_runner.run(maintain_history=True)
self.assertEqual(len(rst.get_histories()), 2*2*2)
rst = benchmark_runner.run_benchmark_config(3, viewer=None, maintain_history=True)
scenario_history = rst.get_histories()[3]
print(scenario_history)
params = ParameterServer()
viewer = MPViewer(
params=params,
x_range=[5060, 5160],
y_range=[5070,5150],
use_world_bounds=True)
viewer.drawWorld(world=scenario_history[1].GetWorldState(),
eval_agent_ids=scenario_history[1].eval_agent_ids)
viewer.show(block=True)
def _test_scenario(self, scenario_idx, num_steps, visualize, viewer):
logging.info("Running scenario {} of {} in set {}".format(scenario_idx,
self._scenario_generator.num_scenarios,
self._set_name))
try:
scenario = self._scenario_generator.get_scenario(scenario_idx)
except Exception as e:
logging.error("Deserialization failed with {}.".format(e))
return False
try:
world_state = scenario.GetWorldState()
except Exception as e:
logging.error("Get world state failed with {}.".format(e))
return False
if visualize:
if not viewer:
viewer = MPViewer(
params=ParameterServer(),
x_range=[5060, 5160],
y_range=[5070, 5150],
use_world_bounds=True)
sim_step_time = 0.2
sim_real_time_factor = 1
try:
for _ in range(0, num_steps): # run a few steps for each scenario
if visualize:
info_text = "SetName: {} | ScenarioIdx: {}".format(
self._set_name, scenario_idx)
viewer.drawText(position=(0.5, 1.05), text=info_text)
viewer.drawWorld(
world_state, scenario._eval_agent_ids, scenario_idx=scenario_idx)
viewer.show(block=False)
time.sleep(sim_step_time/sim_real_time_factor)
viewer.clear()
world_state.Step(sim_step_time)
return True
except Exception as e:
logging.error("Simulation failed with {}.".format(e))
return False
def SimulateWorld(self, world, local_tracer, N=5, **kwargs):
"""Simulates the world for N steps."""
self.ml_behavior.set_action_externally = False
eval_id = self._scenario._eval_agent_ids[0]
self._world.agents[eval_id].behavior_model = self.ml_behavior
replaced_agent_id = kwargs.get("replaced_agent", 0)
if replaced_agent_id not in self._cf_axs and self._visualize_cf_worlds:
self._cf_axs[replaced_agent_id] = {"ax": plt.subplots(3, 1, constrained_layout=True)[1], "count": 0}
for i in range(0, N):
if i == N - 1 and kwargs.get("num_virtual_world", 0) is not None and \
self._visualize_cf_worlds and replaced_agent_id is not None:
# NOTE: outsource
for ftype in [".png", ".pgf"]:
viewer = MPViewer(
params=self._params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True,
axis=self._cf_axs[replaced_agent_id]["ax"][self._cf_axs[replaced_agent_id]["count"]])
# se
for agent_id in world.agents.keys():
viewer.agent_color_map[agent_id] = "gray"
viewer.agent_color_map[replaced_agent_id] = (127/255, 205/255, 187/255)
viewer.agent_color_map[eval_id] = (34/255, 94/255, 168/255)
if replaced_agent_id == 1:
viewer.drawWorld(
world,
eval_agent_ids=self._scenario._eval_agent_ids,
filename=self._results_folder + "cf_%03d_replaced_" % self._count + str(replaced_agent_id)+ftype,
debug_text=False)
self._cf_axs[replaced_agent_id]["count"] += 1
observed_world = world.Observe([eval_id])[0]
eval_state = observed_world.Evaluate()
agent_states = CaptureAgentStates(observed_world)
eval_state = {**eval_state, **agent_states}
# TODO: break at collision
local_tracer.Trace(eval_state, **kwargs)
if eval_state["collision"] or eval_state["drivable_area"]:
break
world.Step(self._step_time)
self.ml_behavior.set_action_externally = True
def test_dr_chn_merging(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/DR_CHN_Merging_ZS_partial_v02.xodr"
))
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
roads = [0, 1]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corridor = map_interface.GetRoadCorridor(roads, driving_direction)
# Draw map
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
viewer.drawPolygon2d(road_corridor.lane_corridors[0].polygon,
color="blue",
alpha=0.5)
viewer.drawPolygon2d(road_corridor.lane_corridors[1].polygon,
color="blue",
alpha=0.5)
viewer.drawPolygon2d(road_corridor.lane_corridors[2].polygon,
color="blue",
alpha=0.5)
viewer.show(block=False)
self.assertTrue(road_corridor.lane_corridors[0].polygon.Valid())
self.assertTrue(road_corridor.lane_corridors[1].polygon.Valid())
self.assertTrue(road_corridor.lane_corridors[2].polygon.Valid())
self.assertTrue(road_corridor.polygon.Valid())
def test_curved_road(self):
params = ParameterServer()
world = World(params)
xodr_map = MakeXodrMapCurved(50, 0.1)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
roads = [100]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corr = map_interface.GetRoadCorridor(roads, driving_direction)
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
viewer.drawRoadCorridor(road_corr)
#viewer.show(block=True)
# if this is not here, the second unit test is not executed (maybe parsing takes too long?)
time.sleep(1)
def test_two_roads_one_lane(self):
params = ParameterServer()
world = World(params)
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
start_point = Point2d(0, -11)
lanes_near_start = map_interface.find_nearest_lanes(start_point, 1)
assert(len(lanes_near_start) == 1)
goal_point = Point2d(-191.789, -50.1725)
lanes_near_goal = map_interface.find_nearest_lanes(goal_point, 1)
assert(len(lanes_near_goal) == 1)
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
# if this is not here, the second unit test is not executed (maybe parsing takes too long?)
time.sleep(2)
ego_agent = get_ego_agent(world)
apply_action_to_ego_agent(world,
0) # applies action with idx 0 (keep velocity)
params = ParameterServer()
viewer = MPViewer(params=params, use_world_bounds=True)
# World Simulation
sim_step_time = params["simulation"]["step_time", "Step-time in simulation",
0.2]
sim_real_time_factor = params["simulation"]["real_time_factor",
"execution in real-time or faster",
1]
# Draw map
video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time)
for _ in range(0, 20):
world.Step(sim_step_time)
viewer.clear()
viewer.drawWorld(world)
viewer.drawGoalDefinition(ego_agent.goal_definition, "red", 0.5, "red")
print(ego_agent.state)
# applies action with idx 1 (break)
apply_action_to_ego_agent(world, 1)
time.sleep(sim_step_time / sim_real_time_factor)
def test_road_corridor_forward(self):
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/road_corridor_test.xodr"))
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
open_drive_map = world.map.GetOpenDriveMap()
viewer = MPViewer(params=params, use_world_bounds=True)
# Draw map
viewer.drawWorld(world)
viewer.show(block=False)
# Generate RoadCorridor
roads = [0, 1, 2]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corridor = map_interface.GetRoadCorridor(roads, driving_direction)
# Assert road corridor
# Assert: 3 roads
self.assertEqual(len(road_corridor.roads), 3)
# Assert: road1: 2 lanes, road2: 1 lane, road3: 1 lane
self.assertEqual(len(road_corridor.GetRoad(0).lanes), 3)
self.assertEqual(len(road_corridor.GetRoad(1).lanes), 2)
self.assertEqual(len(road_corridor.GetRoad(2).lanes), 3)
# Assert: next road
self.assertEqual(road_corridor.GetRoad(0).next_road.road_id, 1)
self.assertEqual(road_corridor.GetRoad(1).next_road.road_id, 2)
# Assert: lane links
self.assertEqual(
road_corridor.GetRoad(0).GetLane(3).next_lane.lane_id, 5)
self.assertEqual(
road_corridor.GetRoad(1).GetLane(5).next_lane.lane_id, 8)
# Assert: LaneCorridor
self.assertEqual(len(road_corridor.lane_corridors), 3)
colors = ["blue", "red", "green"]
count = 0
for lane_corridor in road_corridor.lane_corridors:
viewer.drawPolygon2d(lane_corridor.polygon,
color=colors[count],
alpha=0.5)
viewer.drawLine2d(lane_corridor.left_boundary, color="red")
viewer.drawLine2d(lane_corridor.right_boundary, color="blue")
viewer.drawLine2d(lane_corridor.center_line, color="black")
viewer.show(block=False)
plt.pause(2.)
count += 1
def test_dr_deu_merging_centered(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/DR_DEU_Merging_MT_v01_centered.xodr"
))
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
roads = [0, 1]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corridor = map_interface.GetRoadCorridor(roads, driving_direction)
# Draw map
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
viewer.drawPolygon2d(road_corridor.lane_corridors[0].polygon,
color="blue",
alpha=0.5)
viewer.drawPolygon2d(road_corridor.lane_corridors[1].polygon,
color="blue",
alpha=0.5)
viewer.show(block=False)
self.assertTrue(road_corridor.lane_corridors[0].polygon.Valid())
self.assertTrue(road_corridor.lane_corridors[1].polygon.Valid())
self.assertTrue(road_corridor.polygon.Valid())
tol = 0.2
center_line_array = road_corridor.lane_corridors[
0].center_line.ToArray()
left_boundary_array = road_corridor.lane_corridors[
0].left_boundary.ToArray()
right_boundary_array = road_corridor.lane_corridors[
0].right_boundary.ToArray()
# beginning of left lane
self.assertAlmostEquals(center_line_array[0, 0], 106.4, 1)
self.assertAlmostEquals(center_line_array[0, 1], 103.47, 1)
self.assertAlmostEquals(
Distance(
Point2d(left_boundary_array[0, 0], left_boundary_array[0, 1]),
Point2d(right_boundary_array[0, 0],
right_boundary_array[0, 1])), 2.8, 1)
# end of left lane
self.assertAlmostEquals(center_line_array[-1, 0], -18.15, 1)
self.assertAlmostEquals(center_line_array[-1, 1], 109.0, 1)
self.assertAlmostEquals(
Distance(
Point2d(left_boundary_array[-1, 0], left_boundary_array[-1,
1]),
Point2d(right_boundary_array[-1, 0],
right_boundary_array[-1, 1])), 2.63, 1)