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_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!")