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)