def test_find_lane(self): xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) world.SetMap(map_interface) lane_sw = map_interface.FindLane(Point2d(46, 180)) assert lane_sw.lane_type == XodrLaneType.sidewalk lane_rl = map_interface.FindLane(Point2d(52, 130)) assert lane_rl.lane_type == XodrLaneType.driving lane_no_lane = map_interface.FindLane(Point2d(120, 140)) assert lane_no_lane == None xodr_parser = XodrParser( "modules/runtime/tests/data/city_highway_straight.xodr") np.set_printoptions(precision=8) params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) world.SetMap(map_interface) point = Point2d(5114, 5072) viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.drawPoint2d(point, 'red', 1.0) viewer.show(block=True) time.sleep(0.1) lane_sw = map_interface.FindLane(point) self.assertIsNotNone(lane_sw, "This point is clearly on a lane!")
def test_driving_corridor_splitting_4way_intersection(self): #xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") #xodr_parser = XodrParser("modules/runtime/tests/data/city_highway_straight.xodr") xodr_parser = XodrParser("modules/runtime/tests/data/4way_intersection.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) # xodr_parser.roadgraph.print_graph("/home/esterle/4way_intersection.dot") world.set_map(map_interface) map_interface.compute_all_driving_corridors() all_corridors = map_interface.get_all_corridors() c = all_corridors[11] splittingcorridors = map_interface.get_splitting_corridors(c, [168, 161, 0.0]) assert(len(splittingcorridors) == 0) splittingcorridors = map_interface.get_splitting_corridors(c, [150, 168, 0.0]) assert(len(splittingcorridors) == 2) viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.drawDrivingCorridor(c) if splittingcorridors: for sc in splittingcorridors: viewer.drawDrivingCorridor(sc) viewer.show(block=True) time.sleep(0.1)
def test_driving_corridor_adjacency_4way_intersection(self): #xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") #xodr_parser = XodrParser("modules/runtime/tests/data/city_highway_straight.xodr") xodr_parser = XodrParser("modules/runtime/tests/data/4way_intersection.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) map_interface.set_roadgraph(xodr_parser.roadgraph) #xodr_parser.roadgraph.print_graph("/home/esterle/4way_intersection.dot") world.set_map(map_interface) map_interface.compute_all_driving_corridors() all_corridors = map_interface.get_all_corridors() c = all_corridors[10] right_adj_corridors = map_interface.get_adjacent_corridors_same_direction(c, [151, 168, 0.0]) assert(len(right_adj_corridors) == 2) right_adj_corridors = map_interface.get_adjacent_corridors_same_direction(c, [169, 169, 0.0]) assert(len(right_adj_corridors) == 1) viewer = MPViewer(params=params) viewer.drawWorld(world) viewer.drawDrivingCorridor(c) if right_adj_corridors: for rc in right_adj_corridors: viewer.drawDrivingCorridor(rc) viewer.show(block=True) time.sleep(0.1)
def test_dr_deu_merging(self): # threeway_intersection xodr_parser = XodrParser( "modules/runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.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())
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 test_road_corridor_forward(self): xodr_parser = XodrParser( "modules/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_database_multiprocessing_runner(self): dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=10) 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 > 5 } params = ParameterServer( ) # only for evaluated agents not passed to scenario! behaviors_tested = { "IDM": BehaviorIDMClassic(params), "Const": BehaviorConstantVelocity(params) } benchmark_runner = BenchmarkRunnerMP(benchmark_database=db, evaluators=evaluators, terminal_when=terminal_when, behaviors=behaviors_tested, log_eval_avg_every=10) result = benchmark_runner.run() params2 = ParameterServer() viewer = MPViewer(params=params2, x_range=[5060, 5160], y_range=[5070, 5150], use_world_bounds=True) rst, _ = benchmark_runner.run_benchmark_config(10, viewer=viewer) rst = benchmark_runner.run(maintain_history=True) self.assertEqual(len(rst.get_histories()), 40) rst, scenario_history = benchmark_runner.run_benchmark_config( 11, viewer=None, maintain_history=True) print(scenario_history) viewer = MPViewer(params=params2, x_range=[5060, 5160], y_range=[5070, 5150], use_world_bounds=True) viewer.drawWorld(world=scenario_history[5].GetWorldState(), eval_agent_ids=scenario_history[5].eval_agent_ids) viewer.show(block=True) df = result.get_data_frame() print(df) self.assertEqual( len(df.index), 40) # 2 Behaviors * 10 Serialize Scenarios * 2 scenario sets
def test_two_roads_one_lane(self): xodr_map = MakeXodrMapOneRoadTwoLanes() # World Definition params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_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 = [100] driving_direction = XodrDrivingDirection.forward map_interface.GenerateRoadCorridor(roads, driving_direction) road_corridor = map_interface.GetRoadCorridor(roads, driving_direction) # Assert road corridor # Assert: 1 road self.assertEqual(len(road_corridor.roads), 1) # Assert: road1: 2 lanes self.assertEqual(len(road_corridor.GetRoad(roads[0]).lanes), 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 viewer.show(block=True)
def test_road_corridor_intersection(self): xodr_parser = XodrParser( "modules/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) colors = ["blue", "red", "green", "yellow"] count = 0 for road_id, road in road_corridor.roads.items(): for lane_id, lane in road.lanes.items(): print(road_id, lane_id, lane.driving_direction) 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(0.5) count += 1 viewer.show(block=True)
# Copyright (c) 2019 fortiss GmbH # # This software is released under the MIT License. # https://opensource.org/licenses/MIT from load.benchmark_database import BenchmarkDatabase from modules.runtime.viewer.matplotlib_viewer import MPViewer from modules.runtime.commons.parameters import ParameterServer import time db = BenchmarkDatabase(database_root="external/benchmark_database_release") scenario_generation = db.get_scenario_generator(scenario_set_id=0) param_server = ParameterServer() viewer = MPViewer(params=param_server, x_range=[5060, 5160], y_range=[5070, 5150]) for _ in range(0, 5): # run 5 scenarios in a row, repeating after 3 scenario, idx = scenario_generation[0].get_next_scenario() world_state = scenario.get_world_state() print("Running scenario {} of {}".format( idx, scenario_generation[0].num_scenarios)) for _ in range(0, 10): # run each scenario for 3 steps world_state.step(0.2) viewer.drawWorld(world_state) viewer.show(block=False) time.sleep(0.2)
def test_evaluator_drivable_area(self): # World Definition params = ParameterServer() world = World(params) # Model Definitions behavior_model = BehaviorConstantVelocity(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) # Map Definition map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) #open_drive_map = world.map.GetOpenDriveMap() #agent_2d_shape = CarLimousine() agent_2d_shape = Polygon2d( [1.25, 1, 0], [Point2d(-1, -1), Point2d(-1, 1), Point2d(3, 1), Point2d(3, -1)]) 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), # goal_lane_id map_interface) world.AddAgent(agent) evaluator = EvaluatorDrivableArea() world.AddEvaluator("drivable_area", evaluator) info = world.Evaluate() self.assertFalse(info["drivable_area"]) viewer = MPViewer(params=params, use_world_bounds=True) # Draw map viewer.drawGoalDefinition(goal_polygon, color=(1, 0, 0), alpha=0.5, facecolor=(1, 0, 0)) viewer.drawWorld(world) viewer.drawRoadCorridor(agent.road_corridor) viewer.show(block=False)
open_drive_map = world.map.GetOpenDriveMap() viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.saveFig(output_dir + "/" + "world_plain.png") color_triplet_gray = (0.7,0.7,0.7) # Open Drive Elements (XodrRoads, XodrLane Sections, XodrLanes) for idx_r, road in open_drive_map.GetRoads().items(): viewer.drawWorld(world) viewer.drawXodrRoad(road) viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + ".png") viewer.show(block=False) viewer.clear() for idx_r, road in open_drive_map.GetRoads().items(): for idx_ls, lane_section in enumerate(road.lane_sections): viewer.drawWorld(world) viewer.drawXodrRoad(road, color_triplet_gray) viewer.drawXodrLaneSection(lane_section) viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + "_lane_section" + str(idx_ls) + ".png") viewer.show() viewer.clear() for idx_r, road in open_drive_map.GetRoads().items(): for idx_ls, lane_section in enumerate(road.lane_sections): for idx_l, lane in lane_section.GetLanes().items(): viewer.drawWorld(world)
print("comb_all", comb_all) for cnt, (start_p, end_p) in enumerate(comb_all): polygon = Polygon2d([0, 0, 0], [Point2d(-1,-1),Point2d(-1,1),Point2d(1,1), Point2d(1,-1)]) start_polygon = polygon.Translate(start_p) goal_polygon = polygon.Translate(end_p) rc = map_interface.GenerateRoadCorridor(start_p, goal_polygon) if rc: roads = rc.roads road_ids = list(roads.keys()) print(road_ids, rc.road_ids) viewer.drawWorld(world) viewer.drawRoadCorridor(rc, "blue") viewer.saveFig(output_dir + "/" + "roadcorridor_" + str(cnt) + ".png") viewer.show() viewer.clear() for idx, lane_corridor in enumerate(rc.lane_corridors): viewer.drawWorld(world) viewer.drawLaneCorridor(lane_corridor, "green") viewer.drawLine2d(lane_corridor.left_boundary, color="red") viewer.drawLine2d(lane_corridor.right_boundary, color="green") viewer.drawLine2d(lane_corridor.center_line, color="green") viewer.drawPolygon2d(start_polygon, color="green", facecolor="green", alpha=1.) viewer.drawPolygon2d(goal_polygon, color="red", facecolor="red", alpha=1.) viewer.saveFig(output_dir + "/" + "roadcorridor_" + str(cnt) + "_with_driving_direction_lancecorridor" + str(idx) + ".png") viewer.show() viewer.clear() viewer.show()