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())
