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_line_segment_within_driving_corridor(self): xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) map_interface.compute_all_driving_corridors() all_corridors = map_interface.get_all_corridors() point_sw = Point2d( 46, 180) # this point lies inside the sidewalk left of the road point_rl = Point2d(52, 130) # this point lies in the right lane point_outside = Point2d( 140, 37) # this point lies far outside (in lane of type NONE) point_no_lane = Point2d( 120, 140) # this point lies far outside, not in any lane assert not map_interface.line_segment_inside_corridor( all_corridors[0], point_sw, point_rl) assert not map_interface.line_segment_inside_corridor( all_corridors[0], point_sw, point_no_lane) assert not map_interface.line_segment_inside_corridor( all_corridors[0], point_outside, point_no_lane) assert not map_interface.line_segment_inside_corridor( all_corridors[0], point_sw, point_outside)
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_map(self): xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr") # xodr_parser = XodrParser("modules/runtime/tests/data/CulDeSac.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) world.set_map(map_interface) for _, road in xodr_parser.map.get_roads().items(): for lane_section in road.lane_sections: for _, lane in lane_section.get_lanes().items(): line_np = lane.line.toArray() plt.text(line_np[-1, 0], line_np[-1, 1], 'center_{i}_{j}'.format(i=lane.lane_id,j=lane.lane_position)) plt.plot( line_np[:, 0], line_np[:, 1], color="grey", alpha=1.0) plt.axis("equal") plt.show()
def test_driving_direction(self): xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) point_rl = Point2d(52, 130) # this point lies in the right lane point_ll = Point2d(68, 72) # this point lies in the left lane point_no_lane = Point2d(120, 140) # this point lies far outside, not in any lane assert map_interface.has_correct_driving_direction(point_rl, math.pi/2) assert map_interface.has_correct_driving_direction(point_rl, math.pi/2+0.2) assert map_interface.has_correct_driving_direction(point_rl, math.pi/2-0.2) assert not map_interface.has_correct_driving_direction(point_rl, -math.pi/2) assert map_interface.has_correct_driving_direction(point_ll, -math.pi/4) assert not map_interface.has_correct_driving_direction(point_ll, -math.pi/4 + math.pi) assert not map_interface.has_correct_driving_direction(point_no_lane, 0) assert not map_interface.has_correct_driving_direction(point_no_lane, math.pi/2) assert not map_interface.has_correct_driving_direction(point_no_lane, math.pi)
def test_Crossing8Course(self): xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) world.set_map(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) driving_corridor = map_interface.compute_driving_corridor_from_start_to_goal( lanes_near_start[0].lane_id, lanes_near_goal[0].lane_id) print(driving_corridor) for id in driving_corridor.get_lane_ids(): l = map_interface.get_lane(id[1]) assert(l.lane_type == LaneType.driving) time.sleep(2) # if this is not here, the second unit test is not executed (maybe parsing takes too long?)
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_map(self): xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr") # xodr_parser = XodrParser("modules/runtime/tests/data/CulDeSac.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) world.set_map(map_interface) for _, road in xodr_parser.map.get_roads().items(): for lane_section in road.lane_sections: for _, lane in lane_section.get_lanes().items(): line_np = lane.line.toArray() plt.text(line_np[-1, 0], line_np[-1, 1], 'center_{i}_{j}'.format(i=lane.lane_id,j=lane.lane_position)) plt.plot( line_np[:, 0], line_np[:, 1], color="grey", alpha=1.0) plt.axis("equal") plt.show() # driving corridor calculation test lanes = map_interface.find_nearest_lanes(Point2d(-11,-8),1) left_line, right_line, center_line = map_interface.calculate_driving_corridor(lanes[0].lane_id,2) plt.plot(center_line.toArray()[:,0],center_line.toArray()[:,1]) plt.show()
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 __init__(self): self.carla_server = None self.carla_client = None self.carla_controller = None self.bark_viewer = None self.cosimulation_viewer = None self.launch_args = ["external/carla/CarlaUE4.sh", "-quality-level=Low"] # Bark parameter server self.param_server = ParameterServer( filename=BARK_PATH + "examples/params/od8_const_vel_one_agent.json") # World Definition self.bark_world = World(self.param_server) # Model Definitions self.behavior_model = BehaviorIDMClassic(self.param_server) self.execution_model = ExecutionModelInterpolate(self.param_server) self.dynamic_model = SingleTrackModel(self.param_server) # Map Definition xodr_parser = XodrParser(BARK_PATH + "modules/runtime/tests/data/" + BARK_MAP + ".xodr") self.map_interface = MapInterface() self.map_interface.SetOpenDriveMap(xodr_parser.map) self.bark_world.SetMap(self.map_interface) # Bark agent definition self.agent_2d_shape = CarLimousine() # use for converting carla actor id to bark agent id self.carla_2_bark_id = dict() # store the camera id attached to an agent self.carla_agents_cam = dict()
def CreateMapInterface(self, map_file_name): map_file_load_test = Path(map_file_name) if map_file_load_test.is_file(): xodr_parser = XodrParser(map_file_name) else: objects_found = sorted(Path().rglob(map_file_name)) if len(objects_found) == 0: raise ValueError("No Map found") elif len(objects_found) > 1: raise ValueError("Multiple Maps found") else: xodr_parser = XodrParser(objects_found[0].as_posix()) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) self._map_interface = map_interface
def setup_map(self, world, _map_file_name): xodr_parser = XodrParser(_map_file_name ) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) map_interface.set_roadgraph(xodr_parser.roadgraph) world.set_map(map_interface) return world
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 setup_map(self, world, _map_file_name): if not _map_file_name: return world xodr_parser = XodrParser(_map_file_name) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) self._map_interface = map_interface world.SetMap(map_interface) return world
def setup_map(self, world, _map_file_name): if not _map_file_name: return world map_file_load_test = Path(_map_file_name) if map_file_load_test.is_file(): xodr_parser = XodrParser(_map_file_name) else: objects_found = sorted(Path().rglob(_map_file_name)) if len(objects_found) == 0: raise ValueError("No Map found") elif len(objects_found) > 1: raise ValueError("Multiple Maps found") else: xodr_parser = XodrParser(objects_found[0].as_posix()) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) self._map_interface = map_interface world.SetMap(map_interface) return world
def test_map(self): xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") # xodr_parser = XodrParser("modules/runtime/tests/data/CulDeSac.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) world.set_map(map_interface) for _, road in xodr_parser.map.get_roads().items(): for lane_section in road.lane_sections: for _, lane in lane_section.get_lanes().items(): if lane.lane_type == LaneType.driving: color = "grey" elif lane.lane_type == LaneType.sidewalk: color = "green" else: continue line_np = lane.line.toArray() plt.text( line_np[-1, 0], line_np[-1, 1], 'center_{i}_{j}'.format(i=lane.lane_id, j=lane.lane_position)) plt.plot(line_np[:, 0], line_np[:, 1], color=color, alpha=1.0) plt.axis("equal") plt.show() # driving corridor calculation test #lanes = map_interface.find_nearest_lanes(Point2d(-11,-8),1) #left_line, right_line, center_line = map_interface.calculate_driving_corridor(lanes[0].lane_id,2) #plt.plot(center_line.toArray()[:,0],center_line.toArray()[:,1]) #plt.show() # TODO: plot cpp map #cwd = os.getcwd() #print (cwd) roadgraph = xodr_parser.roadgraph roadgraph.print_graph("/home/bernhard/" + "test1234.dot")
def test_between_lanes(self): 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) # Simple test point_close = Point2d(5114.68262, 5086.44971) lane_sw = map_interface.FindLane(point_close) self.assertIsNotNone( lane_sw, "This point is still in the left lane! XodrLane boundary is 5114.683" ) switched_lane = False lng_coord = 5086.44971 i = 5114.5 lane_sw = map_interface.FindLane(Point2d(i, lng_coord)) assert lane_sw != None prev = lane_sw.lane_id prev_i = i while (i < 5117.5): lane_sw = map_interface.FindLane(Point2d(i, lng_coord)) self.assertIsNotNone( lane_sw, "Should always be on at least one lane! Currently at ({}, {})". format(i, lng_coord)) if prev != lane_sw.lane_id: # print(prev) # print(prev_i) # print(lane_sw.lane_id) # print(i) self.assertFalse(switched_lane, "XodrLane switch should only happens once!") switched_lane = True prev_i = i prev = lane_sw.lane_id i = i + 0.01 self.assertTrue(switched_lane, "Eventually should have switched lanes!")
def test_map_CulDeSac(self): xodr_parser = XodrParser("modules/runtime/tests/data/CulDeSac.xodr") #dot_file_path = "/home/esterle/roadgraph/" + "CulDeSac_temp.dot" dot_file_path = "CulDeSac.dot" params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) #helper_plot(xodr_parser) roadgraph = map_interface.get_roadgraph() roadgraph.print_graph(dot_file_path) self.assertTrue(filecmp.cmp("modules/runtime/tests/data/CulDeSac_ideal.dot", dot_file_path, shallow=False))
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.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) lane_sw = map_interface.find_lane(Point2d(46, 180)) assert lane_sw.lane_type == LaneType.sidewalk lane_rl = map_interface.find_lane(Point2d(52, 130)) assert lane_rl.lane_type == LaneType.driving lane_no_lane = map_interface.find_lane(Point2d(120, 140)) assert lane_no_lane == None
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)
def test_Crossing8Course(self): xodr_parser = XodrParser( "modules/runtime/tests/data/Crossing8Course.xodr") params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.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) time.sleep( 2 ) # if this is not here, the second unit test is not executed (maybe parsing takes too long?)
def test_map_4way_intersection(self): xodr_parser = XodrParser( "modules/runtime/tests/data/4way_intersection.xodr") map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map)
param_server = ParameterServer() # World Definition world = World(param_server) # Model Definitions behavior_model = BehaviorConstantVelocity(param_server) execution_model = ExecutionModelInterpolate(param_server) dynamic_model = SingleTrackModel() behavior_model2 = BehaviorConstantVelocity(param_server) execution_model2 = ExecutionModelInterpolate(param_server) dynamic_model2 = SingleTrackModel() # Map Definition xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr") map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) map_interface.set_roadgraph(xodr_parser.roadgraph) world.set_map(map_interface) # Agent Definition agent_2d_shape = CarLimousine() init_state = np.array([0, -11, -8, 3.14 * 3.0 / 4.0, 50 / 3.6]) goal_polygon = Polygon2d( [0, 0, 0], [Point2d(-1, -1), Point2d(-1, 1), Point2d(1, 1), Point2d(1, -1)]) goal_polygon = goal_polygon.translate(Point2d(-63, -61))
def test_map_Crossing8(self): xodr_parser = XodrParser( "modules/runtime/tests/data/Crossing8Course.xodr") map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map)
def test_map_urban_road(self): xodr_parser = XodrParser("modules/runtime/tests/data/urban_road.xodr") map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map)
def test_map_DR_DEU_Merging_MT_v01_shifted(self): xodr_parser = XodrParser( "modules/runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.xodr") map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map)
def test_map_highway(self): xodr_parser = XodrParser( "modules/runtime/tests/data/city_highway_straight.xodr") map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map)
def test_three_way_intersection(self): # threeway_intersection xodr_parser = XodrParser( "modules/runtime/tests/data/threeway_intersection.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) comb_all = [] start_point = [Point2d(-30, -2)] end_point_list = [Point2d(30, -2), Point2d(-2, -30)] comb = list(itertools.product(start_point, end_point_list)) comb_all = comb_all + comb # starting on the right start_point = [Point2d(30, 2)] end_point_list = [Point2d(-30, 2)] comb = list(itertools.product(start_point, end_point_list)) comb_all = comb_all + comb # starting on the bottom start_point = [Point2d(2, -30)] end_point_list = [Point2d(30, -2), Point2d(-30, 2)] comb = list(itertools.product(start_point, end_point_list)) comb_all = comb_all + comb # check few corridors def GenerateRoadCorridor(map_interface, comb): (start_p, end_p) = comb 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) return rc # assert road ids rc = GenerateRoadCorridor(map_interface, comb_all[0]) self.assertEqual(rc.road_ids, [0, 11, 1]) self.assertEqual(len(rc.lane_corridors), 3) rc = GenerateRoadCorridor(map_interface, comb_all[1]) self.assertEqual(rc.road_ids, [0, 5, 2]) self.assertEqual(len(rc.lane_corridors), 3) rc = GenerateRoadCorridor(map_interface, comb_all[2]) self.assertEqual(rc.road_ids, [1, 10, 0]) self.assertEqual(len(rc.lane_corridors), 3) rc = GenerateRoadCorridor(map_interface, comb_all[3]) self.assertEqual(rc.road_ids, [2, 6, 1]) self.assertEqual(len(rc.lane_corridors), 3) rc = GenerateRoadCorridor(map_interface, comb_all[4]) self.assertEqual(rc.road_ids, [2, 4, 0]) self.assertEqual(len(rc.lane_corridors), 3)
def test_FindRoadPath_4way_intersection(self): xodr_parser = XodrParser( "modules/runtime/tests/data/4way_intersection.xodr") rg = Roadgraph() rg.Generate(xodr_parser.map) road_path = rg.FindRoadPath(2, 2) self.assertEqual(len(road_path), 1) road_path = rg.FindRoadPath(2, 8) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 2) self.assertEqual(road_path[1], 5) self.assertEqual(road_path[2], 8) road_path = rg.FindRoadPath(2, 3) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 2) self.assertEqual(road_path[1], 10) self.assertEqual(road_path[2], 3) road_path = rg.FindRoadPath(2, 15) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 2) self.assertEqual(road_path[1], 17) self.assertEqual(road_path[2], 15) road_path = rg.FindRoadPath(8, 15) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 8) self.assertEqual(road_path[1], 14) self.assertEqual(road_path[2], 15) road_path = rg.FindRoadPath(8, 2) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 8) self.assertEqual(road_path[1], 7) self.assertEqual(road_path[2], 2) road_path = rg.FindRoadPath(8, 3) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 8) self.assertEqual(road_path[1], 4) self.assertEqual(road_path[2], 3) road_path = rg.FindRoadPath(3, 2) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 3) self.assertEqual(road_path[1], 9) self.assertEqual(road_path[2], 2) road_path = rg.FindRoadPath(3, 15) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 3) self.assertEqual(road_path[1], 12) self.assertEqual(road_path[2], 15) road_path = rg.FindRoadPath(3, 8) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 3) self.assertEqual(road_path[1], 6) self.assertEqual(road_path[2], 8) road_path = rg.FindRoadPath(15, 2) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 15) self.assertEqual(road_path[1], 11) self.assertEqual(road_path[2], 2) road_path = rg.FindRoadPath(15, 8) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 15) self.assertEqual(road_path[1], 13) self.assertEqual(road_path[2], 8) road_path = rg.FindRoadPath(15, 3) self.assertEqual(len(road_path), 3) self.assertEqual(road_path[0], 15) self.assertEqual(road_path[1], 16) self.assertEqual(road_path[2], 3)
import filecmp import matplotlib.pyplot as plt from bark.world import World from bark.world.map import MapInterface from modules.runtime.commons.parameters import ParameterServer from modules.runtime.commons.xodr_parser import XodrParser from modules.runtime.viewer.matplotlib_viewer import MPViewer import numpy as np # Name and Output Directory # CHANGE THIS # map_name = "4way_intersection" output_dir = "/home/esterle/map_analysis" + map_name # Map Definition xodr_parser = XodrParser("modules/runtime/tests/data/" + map_name + ".xodr") if not os.path.exists(output_dir): os.makedirs(output_dir) # World Definition params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) open_drive_map = world.map.get_open_drive_map() viewer = MPViewer(params=params, use_world_bounds=True)