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)