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_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_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)
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 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.set_open_drive_map(xodr_parser.map) world.set_map(map_interface) point = Point2d(5111, 5072) viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) polygon = world.map.get_roadgraph().get_lane_polygon_by_id(241) polygon2 = world.map.get_roadgraph().get_lane_polygon_by_id(242) viewer.drawPolygon2d(polygon, 'blue', 1.0) viewer.drawPolygon2d(polygon2, 'green', 1.0) viewer.drawPoint2d(point, 'red', 1.0) viewer.show(block=True) time.sleep(0.1) lane_sw = map_interface.find_lane(point) self.assertIsNotNone(lane_sw, "This point is clearly on a lane!")
viewer.drawLane(lane) viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + "_lane_section" + str(idx_ls) + "_lane" + str(idx_l) + ".png") viewer.clear() # Lanes of Roadgraph roadgraph = map_interface.get_roadgraph() roadgraph.print_graph(output_dir + "/" + map_name) lane_ids = roadgraph.get_all_laneids() for lane_id in lane_ids: lane_polygon = roadgraph.get_lane_polygon_by_id(lane_id) viewer.drawWorld(world) color = list(np.random.choice(range(256), size=3) / 256) viewer.drawPolygon2d(lane_polygon, color, 1.0) viewer.saveFig(output_dir + "/" + "roadgraph_laneid_" + str(lane_id) + ".png") viewer.clear() #for rc in all_corridors: # viewer.drawDrivingCorridor(rc) # viewer.saveFig(output_dir + "/" + "test.png") #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)
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() viewer.clear() def DrawRoadCorridor(road_ids, dr=XodrDrivingDirection.forward): map_interface.GenerateRoadCorridor(road_ids, dr) rc = map_interface.GetRoadCorridor(road_ids, dr) if rc: roads = rc.roads road_ids = list(roads.keys())