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)