def test_map_DR_DEU_Merging_MT_v01_shifted(self):
xodr_parser = XodrParser(
os.path.join(os.path.dirname(__file__),
"data/DR_DEU_Merging_MT_v01_shifted.xodr"))
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
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 test_map_highway(self):
xodr_parser = XodrParser(
os.path.join(os.path.dirname(__file__),
"data/city_highway_straight.xodr"))
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
def test_between_lanes(self):
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../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(2, -92.55029)
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 = -92.55029
i = 1.817
lane_sw = map_interface.FindLane(Point2d(i, lng_coord))
assert lane_sw != None
prev = lane_sw.lane_id
prev_i = i
while (i < 4.817):
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_dr_chn_merging(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/DR_CHN_Merging_ZS_partial_v02.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.drawPolygon2d(road_corridor.lane_corridors[2].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.lane_corridors[2].polygon.Valid())
self.assertTrue(road_corridor.polygon.Valid())
def load_map(map):
xodr_parser = XodrParser(map)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
return map_interface
from bark.core.geometry import Point2d, Polygon2d
from bark.core.world.opendrive import XodrDrivingDirection
import bark
bark.core.commons.GLogInit("", "./", 5, True, "")
import numpy as np
import itertools
# Name and Output Directory
# CHANGE THIS #
map_name = "left_turn_rural"
output_dir = "/tmp/" + map_name
# Map Definition
xodr_parser = XodrParser("bark/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.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
open_drive_map = world.map.GetOpenDriveMap()
viewer = MPViewer(params=params, use_world_bounds=True)
def test_three_way_intersection(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../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)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[1])
self.assertEqual(rc.road_ids, [0, 5, 2])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[2])
self.assertEqual(rc.road_ids, [1, 10, 0])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[3])
self.assertEqual(rc.road_ids, [2, 6, 1])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[4])
self.assertEqual(rc.road_ids, [2, 4, 0])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
def test_road_corridor_forward(self):
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../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_dr_deu_merging_centered(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/DR_DEU_Merging_MT_v01_centered.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())
tol = 0.2
center_line_array = road_corridor.lane_corridors[
0].center_line.ToArray()
left_boundary_array = road_corridor.lane_corridors[
0].left_boundary.ToArray()
right_boundary_array = road_corridor.lane_corridors[
0].right_boundary.ToArray()
# beginning of left lane
self.assertAlmostEquals(center_line_array[0, 0], 106.4, 1)
self.assertAlmostEquals(center_line_array[0, 1], 103.47, 1)
self.assertAlmostEquals(
Distance(
Point2d(left_boundary_array[0, 0], left_boundary_array[0, 1]),
Point2d(right_boundary_array[0, 0],
right_boundary_array[0, 1])), 2.8, 1)
# end of left lane
self.assertAlmostEquals(center_line_array[-1, 0], -18.15, 1)
self.assertAlmostEquals(center_line_array[-1, 1], 109.0, 1)
self.assertAlmostEquals(
Distance(
Point2d(left_boundary_array[-1, 0], left_boundary_array[-1,
1]),
Point2d(right_boundary_array[-1, 0],
right_boundary_array[-1, 1])), 2.63, 1)
