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