Example #1
0
    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())
Example #2
0
    def test_curved_road(self):
        params = ParameterServer()
        world = World(params)

        xodr_map = MakeXodrMapCurved(50, 0.1)

        map_interface = MapInterface()
        map_interface.SetOpenDriveMap(xodr_map)
        world.SetMap(map_interface)

        roads = [100]
        driving_direction = XodrDrivingDirection.forward
        map_interface.GenerateRoadCorridor(roads, driving_direction)
        road_corr = map_interface.GetRoadCorridor(roads, driving_direction)

        viewer = MPViewer(params=params, use_world_bounds=True)
        viewer.drawWorld(world)
        viewer.drawRoadCorridor(road_corr)
        #viewer.show(block=True)

        # if this is not here, the second unit test is not executed (maybe parsing takes too long?)
        time.sleep(1)
Example #3
0
# end_point_list = [Point2d(153, 172), Point2d(168, 153), Point2d(188, 168)]
# comb = list(itertools.product(start_point, end_point_list))
# comb_all = comb_all + comb

print("comb_all", comb_all)

for cnt, (start_p, end_p) in enumerate(comb_all):
    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)
    if rc:
        roads = rc.roads
        road_ids = list(roads.keys())
        print(road_ids, rc.road_ids)

        viewer.drawWorld(world)
        viewer.drawRoadCorridor(rc, "blue")
        viewer.saveFig(output_dir + "/" + "roadcorridor_" + str(cnt) + ".png")
        viewer.show()
        viewer.clear()

        for idx, lane_corridor in enumerate(rc.lane_corridors):
            viewer.drawWorld(world)
            viewer.drawLaneCorridor(lane_corridor, "green")
            viewer.drawLine2d(lane_corridor.left_boundary, color="red")
Example #4
0
    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
Example #5
0
    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)